From 801bc71402f3f99c0874fb26711960f6212ebcee Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 28 Jun 2019 15:02:47 +0800 Subject: [PATCH 01/91] Update tests --- tests/testNonlinearOptimizer.cpp | 47 ++++++++++++++------------------ 1 file changed, 20 insertions(+), 27 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0e77935527..737c2f76d7 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -52,7 +52,7 @@ using symbol_shorthand::L; TEST( NonlinearOptimizer, iterateLM ) { // really non-linear factor graph - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); // config far from minimum Point2 x0(3,0); @@ -60,8 +60,7 @@ TEST( NonlinearOptimizer, iterateLM ) config.insert(X(1), x0); // normal iterate - GaussNewtonParams gnParams; - GaussNewtonOptimizer gnOptimizer(fg, config, gnParams); + GaussNewtonOptimizer gnOptimizer(fg, config); gnOptimizer.iterate(); // LM iterate with lambda 0 should be the same @@ -76,7 +75,7 @@ TEST( NonlinearOptimizer, iterateLM ) /* ************************************************************************* */ TEST( NonlinearOptimizer, optimize ) { - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); // test error at minimum Point2 xstar(0,0); @@ -116,7 +115,7 @@ TEST( NonlinearOptimizer, optimize ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleLMOptimizer ) { - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); Values c0; @@ -129,7 +128,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleGNOptimizer ) { - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); Values c0; @@ -142,7 +141,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) /* ************************************************************************* */ TEST( NonlinearOptimizer, SimpleDLOptimizer ) { - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); Values c0; @@ -153,24 +152,21 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) } /* ************************************************************************* */ -TEST( NonlinearOptimizer, optimization_method ) -{ - LevenbergMarquardtParams paramsQR; - paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; - LevenbergMarquardtParams paramsChol; - paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; +TEST(NonlinearOptimizer, optimization_method) { + auto fg = example::createReallyNonlinearFactorGraph(); - NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); - - Point2 x0(3,3); + Point2 x0(3, 3); Values c0; c0.insert(X(1), x0); - Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); - DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); + LevenbergMarquardtParams params; + params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; + Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); - Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize(); - DOUBLES_EQUAL(0,fg.error(actualMFChol),tol); + params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; + Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); } /* ************************************************************************* */ @@ -202,7 +198,7 @@ TEST( NonlinearOptimizer, Factorization ) /* ************************************************************************* */ TEST(NonlinearOptimizer, NullFactor) { - NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + auto fg = example::createReallyNonlinearFactorGraph(); // Add null factor fg.push_back(NonlinearFactorGraph::sharedFactor()); @@ -263,7 +259,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { expectedGradient.insert(2,Z_3x1); // Try LM and Dogleg - LevenbergMarquardtParams params = LevenbergMarquardtParams::LegacyDefaults(); + auto params = LevenbergMarquardtParams::LegacyDefaults(); { LevenbergMarquardtOptimizer optimizer(fg, init, params); @@ -277,8 +273,6 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { } EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); -// cout << "===================================================================================" << endl; - // Try LM with diagonal damping Values initBetter; initBetter.insert(0, Pose2(3,4,0)); @@ -360,9 +354,8 @@ TEST(NonlinearOptimizer, MoreOptimizationWithHuber) { expected.insert(1, Pose2(1,0,M_PI/2)); expected.insert(2, Pose2(1,1,M_PI)); - LevenbergMarquardtParams params; EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize())); - EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize())); + EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize())); EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize())); } @@ -437,7 +430,7 @@ TEST(NonlinearOptimizer, subclass_solver) { #include TEST( NonlinearOptimizer, logfile ) { - NonlinearFactorGraph fg(example::createReallyNonlinearFactorGraph()); + auto fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); Values c0; From 15279c332a9e6486335b4363281ef9f1b6ec6fd5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 30 Jun 2019 14:08:06 +0800 Subject: [PATCH 02/91] Update creation of triplets and test --- gtsam/linear/GaussianFactorGraph.cpp | 18 ++++++------ .../linear/tests/testGaussianFactorGraph.cpp | 29 ++++++++++--------- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 8dc3600c68..c34bc2a33d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -102,8 +102,7 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable - typedef std::map KeySizeMap; - KeySizeMap dims; + std::map dims; for (const sharedFactor& factor : *this) { if (!static_cast(factor)) continue; @@ -116,10 +115,10 @@ namespace gtsam { // Compute first scalar column of each variable size_t currentColIndex = 0; - KeySizeMap columnIndices = dims; - for (const KeySizeMap::value_type& col : dims) { - columnIndices[col.first] = currentColIndex; - currentColIndex += dims[col.first]; + std::map columnIndices; + for (const auto& keyValue : dims) { + columnIndices[keyValue.first] = currentColIndex; + currentColIndex += keyValue.second; } // Iterate over all factors, adding sparse scalar entries @@ -154,19 +153,20 @@ namespace gtsam { for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { double s = whitenedA(i, j); if (std::abs(s) > 1e-12) - entries.push_back(boost::make_tuple(row + i, column_start + j, s)); + entries.emplace_back(row + i, column_start + j, s); } } JacobianFactor::constBVector whitenedb(whitened.getb()); size_t bcolumn = currentColIndex; for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i))); + entries.emplace_back(row + i, bcolumn, whitenedb(i)); // Increment row index row += jacobianFactor->rows(); } - return vector(entries.begin(), entries.end()); + + return entries; } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 45f652d056..b480871ec2 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testGaussianFactorGraphUnordered.cpp - * @brief Unit tests for Linear Factor Graph + * @file testGaussianFactorGraph.cpp + * @brief Unit tests for Gaussian (i.e., Linear) Factor Graph * @author Christian Potthast * @author Frank Dellaert * @author Luca Carlone @@ -73,8 +73,17 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 + GaussianFactorGraph gfg; + SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); + gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); + gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1, + (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model); + + // Check the triplets size... + auto entries = gfg.sparseJacobian(); + EXPECT_LONGS_EQUAL(16, entries.size()); - // Expected - NOTE that we transpose this! + // Check version for MATLAB - NOTE that we transpose this! Matrix expectedT = (Matrix(16, 3) << 1., 1., 2., 1., 2., 4., @@ -92,18 +101,10 @@ TEST(GaussianFactorGraph, sparseJacobian) { 4., 5.,30., 3., 6.,26., 4., 6.,32.).finished(); + Matrix expectedMatlab = expectedT.transpose(); + Matrix matlab = gfg.sparseJacobian_(); - Matrix expected = expectedT.transpose(); - - GaussianFactorGraph gfg; - SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); - gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1, - (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model); - - Matrix actual = gfg.sparseJacobian_(); - - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(expectedMatlab, matlab)); } /* ************************************************************************* */ From 21a13edf2f7280e16cf5a5faa50435fa8b41976f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 2 Jul 2019 18:54:08 +0800 Subject: [PATCH 03/91] Removed redundant Scatter method --- gtsam/linear/Scatter.cpp | 15 ++++++--------- gtsam/linear/Scatter.h | 3 --- gtsam/nonlinear/NonlinearFactorGraph.cpp | 17 +++++++++++++---- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 07ecaf4838..75385e3ac8 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -41,8 +41,10 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, gttic(Scatter_Constructor); // If we have an ordering, pre-fill the ordered variables first - for (Key key : ordering) { - add(key, 0); + if (ordering) { + for (Key key : *ordering) { + emplace_back(key, 0); + } } // Now, find dimensions of variables and/or extend @@ -54,7 +56,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, const JacobianFactor* asJacobian = dynamic_cast(factor.get()); if (asJacobian && asJacobian->cols() <= 1) continue; - // loop over variables + // loop over variable keys for (GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { const Key key = *variable; @@ -62,7 +64,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, if (it!=end()) it->dimension = factor->getDim(variable); else - add(key, factor->getDim(variable)); + emplace_back(key, factor->getDim(variable)); } } @@ -72,11 +74,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, if (first != end()) std::sort(first, end()); } -/* ************************************************************************* */ -void Scatter::add(Key key, size_t dim) { - emplace_back(SlotEntry(key, dim)); -} - /* ************************************************************************* */ FastVector::iterator Scatter::find(Key key) { iterator it = begin(); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index b05d191bc9..9490df4c9a 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -57,9 +57,6 @@ class Scatter : public FastVector { /// Construct from gaussian factor graph, with (partial or complete) ordering GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); - /// Add a key/dim pair - GTSAM_EXPORT void add(Key key, size_t dim); - private: /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index d4b9fbb68e..806a653b32 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,15 +344,24 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values) { +static Scatter scatterFromValues(const Values& values, + boost::optional ordering) { gttic(scatterFromValues); Scatter scatter; scatter.reserve(values.size()); - // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); + if (!ordering) { + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.emplace_back(key_value.key, key_value.value.dim()); + } + } else { + // copy ordering into keys and lookup dimension in values, is O(n*log n) + for (Key key : *ordering) { + const Value& value = values.at(key); + scatter.emplace_back(key, value.dim()); + } } return scatter; From f0f250f6012463f0df38ab95b0ad52d838ee8884 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 2 Aug 2019 11:22:09 -0400 Subject: [PATCH 04/91] Sparse Eigen solver prototype --- gtsam/nonlinear/NonlinearOptimizer.cpp | 52 +++++++++++++++++++--- gtsam/nonlinear/NonlinearOptimizerParams.h | 5 +++ tests/testNonlinearOptimizer.cpp | 11 +++++ 3 files changed, 61 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0c5d99c0f6..d3335fcf68 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -27,6 +27,8 @@ #include +#include + #include #include @@ -124,8 +126,9 @@ const Values& NonlinearOptimizer::optimizeSafely() { } /* ************************************************************************* */ -VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, - const NonlinearOptimizerParams& params) const { +VectorValues NonlinearOptimizer::solve( + const GaussianFactorGraph& gfg, + const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point VectorValues delta; @@ -147,22 +150,57 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) - throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); if (boost::shared_ptr pcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + boost::dynamic_pointer_cast( + params.iterativeParams)) { delta = PCGSolver(*pcg).optimize(gfg); } else if (boost::shared_ptr spcg = - boost::dynamic_pointer_cast(params.iterativeParams)) { + boost::dynamic_pointer_cast( + params.iterativeParams)) { if (!params.ordering) throw std::runtime_error("SubgraphSolver needs an ordering"); delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); } else { throw std::runtime_error( - "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); + "NonlinearOptimizer::solve: special cg parameter type is not handled " + "in LM solver ..."); } + } else if (params.isEigen()) { + // TODO(Mandy): make this a GFG method ? + + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = gfg.sparseJacobian(); + + // Convert boost tuples to Eigen triplets + // TODO(Mandy): Levenberg-MQ prior has zeros in RHS + vector> triplets; + entries.reserve(triplets.size()); + for (const auto& e : entries) { + triplets.emplace_back(e.get<0>(), e.get<1>(), e.get<2>()); + cout << e.get<0>() << ", " << e.get<1>()<< ", " << e.get<2>() << endl; + } + + // ...and make a sparse matrix with it. + const auto& lastTriplet = triplets.back(); + const size_t rows = lastTriplet.row() + 1, cols = lastTriplet.col(); + using SpMat = Eigen::SparseMatrix; + SpMat Ab(rows, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + cout << Ab << endl; + + // Solve A*x = b using sparse QR from Eigen + Eigen::SparseQR> qr(Ab.rightCols(cols)); + Eigen::VectorXd x = qr.solve(Ab.col(cols)); + cout << x << endl; + + // TODO(Mandy): make delta } else { - throw std::runtime_error("NonlinearOptimizer::solve: Optimization parameter is invalid"); + throw std::runtime_error( + "NonlinearOptimizer::solve: Optimization parameter is invalid"); } // return update diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 65fdd1c92f..ef425e5db8 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -79,6 +79,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams { SEQUENTIAL_QR, Iterative, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ + EIGEN, }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer @@ -103,6 +104,10 @@ class GTSAM_EXPORT NonlinearOptimizerParams { return (linearSolverType == Iterative); } + inline bool isEigen() const { + return (linearSolverType == EIGEN); + } + GaussianFactorGraph::Eliminate getEliminationFunction() const { switch (linearSolverType) { case MULTIFRONTAL_CHOLESKY: diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 737c2f76d7..2f7a83d9ef 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -153,20 +153,31 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) /* ************************************************************************* */ TEST(NonlinearOptimizer, optimization_method) { + // Create nonlinear example auto fg = example::createReallyNonlinearFactorGraph(); + // Create some test Values (just one 2D point, in this case) Point2 x0(3, 3); Values c0; c0.insert(X(1), x0); + // Below we solve with different backend linear solver choices LevenbergMarquardtParams params; + + // Multifrontal QR, will be parallel if TBB installed params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); + // Multifrontal Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); + + // Test sparse Eigen solver + params.linearSolverType = LevenbergMarquardtParams::EIGEN; + Values actualEigen = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actualEigen), tol); } /* ************************************************************************* */ From 877904c996dd363242cc6784236de36f4321f819 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 2 Aug 2019 12:27:39 -0400 Subject: [PATCH 05/91] Use emplace for LM prior --- gtsam/nonlinear/internal/LevenbergMarquardtState.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h index 8ab2e74663..ff88ce008a 100644 --- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h +++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h @@ -130,7 +130,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const Key key = key_value.key; const size_t dim = key_value.value.dim(); const CachedModel* item = getCachedModel(dim); - damped += boost::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } return damped; } @@ -146,7 +146,7 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState { const size_t dim = key_vector.second.size(); CachedModel* item = getCachedModel(dim); item->A.diagonal() = sqrtHessianDiagonal.at(key); // use diag(hessian) - damped += boost::make_shared(key, item->A, item->b, item->model); + damped.emplace_shared(key, item->A, item->b, item->model); } catch (const std::out_of_range&) { continue; // Don't attempt any damping if no key found in diagonal } From 953c4ec8affea95253ee1de4af734e5ce69ee2db Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 2 Aug 2019 12:27:58 -0400 Subject: [PATCH 06/91] Modernize example to c+11 standards --- tests/smallExample.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/tests/smallExample.h b/tests/smallExample.h index 2b29a6d101..d1dca6f6b6 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -347,15 +347,14 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1 { } /* ************************************************************************* */ -inline boost::shared_ptr sharedReallyNonlinearFactorGraph() { +inline boost::shared_ptr +sharedReallyNonlinearFactorGraph() { using symbol_shorthand::X; - using symbol_shorthand::L; - boost::shared_ptr fg(new NonlinearFactorGraph); + auto fg = boost::make_shared(); Point2 z(1.0, 0.0); double sigma = 0.1; - boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); - fg->push_back(factor); + auto model = noiseModel::Isotropic::Sigma(2, sigma); + fg->emplace_shared(z, model, X(1)); return fg; } From 7ad3875c2b01664b243dc5996d9f86c87649658c Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sat, 3 Aug 2019 10:50:26 -0400 Subject: [PATCH 07/91] fix bug in using SparseQR of Eigen --- gtsam/nonlinear/NonlinearOptimizer.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index d3335fcf68..2651135069 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -193,10 +193,23 @@ VectorValues NonlinearOptimizer::solve( cout << Ab << endl; // Solve A*x = b using sparse QR from Eigen - Eigen::SparseQR> qr(Ab.rightCols(cols)); + Eigen::SparseQR> qr(Ab); Eigen::VectorXd x = qr.solve(Ab.col(cols)); cout << x << endl; + // First find dimensions of each variable + std::map dims; + for (const auto& factor : gfg) { + if (!static_cast(factor)) + continue; + + for (GaussianFactor::const_iterator key = factor->begin(); + key != factor->end(); ++key) { + dims[*key] = factor->getDim(key); + } + } + delta = VectorValues(x, dims); + // TODO(Mandy): make delta } else { throw std::runtime_error( From 47b53c73bd7d9c6b92625100de2a68ca76bf843b Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sat, 3 Aug 2019 10:51:01 -0400 Subject: [PATCH 08/91] test Eigen with GaussNewton --- tests/testNonlinearOptimizer.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 2f7a83d9ef..45b116a412 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -162,21 +162,21 @@ TEST(NonlinearOptimizer, optimization_method) { c0.insert(X(1), x0); // Below we solve with different backend linear solver choices - LevenbergMarquardtParams params; + GaussNewtonParams params; // Multifrontal QR, will be parallel if TBB installed - params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; - Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + params.linearSolverType = GaussNewtonParams::MULTIFRONTAL_QR; + Values actualMFQR = GaussNewtonOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); // Multifrontal Cholesky (more sensitive to conditioning, but faster) - params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; - Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + params.linearSolverType = GaussNewtonParams::MULTIFRONTAL_CHOLESKY; + Values actualMFChol = GaussNewtonOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); // Test sparse Eigen solver - params.linearSolverType = LevenbergMarquardtParams::EIGEN; - Values actualEigen = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + params.linearSolverType = GaussNewtonParams::EIGEN; + Values actualEigen = GaussNewtonOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigen), tol); } From b1dd8db5a59eb83e87db60621a88335acd34d281 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sun, 4 Aug 2019 16:06:50 -0400 Subject: [PATCH 09/91] fix problem:y zero terms in sparseJacobian --- gtsam/linear/GaussianFactorGraph.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index c34bc2a33d..9350ac85e7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -30,6 +30,8 @@ #include #include +#include + using namespace std; using namespace gtsam; @@ -159,8 +161,11 @@ namespace gtsam { JacobianFactor::constBVector whitenedb(whitened.getb()); size_t bcolumn = currentColIndex; - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) - entries.emplace_back(row + i, bcolumn, whitenedb(i)); + for (size_t i = 0; i < (size_t) whitenedb.size(); i++) { + double s = whitenedb(i); + if (std::abs(s) > 1e-12) + entries.emplace_back(row + i, bcolumn, s); + } // Increment row index row += jacobianFactor->rows(); @@ -500,5 +505,4 @@ namespace gtsam { const Eliminate& function) const { return optimize(function); } - } // namespace gtsam From 3be59ed4c3804e562c712061edfb742c5a12d086 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sun, 4 Aug 2019 16:09:18 -0400 Subject: [PATCH 10/91] add eigenSparseQR method to solve Ax=b --- gtsam/linear/GaussianFactorGraph.cpp | 28 ++++++++++++++++++++++++++++ gtsam/linear/GaussianFactorGraph.h | 3 +++ 2 files changed, 31 insertions(+) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 9350ac85e7..3591999f05 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -505,4 +505,32 @@ namespace gtsam { const Eliminate& function) const { return optimize(function); } + + VectorValues GaussianFactorGraph::eigenSparseQR() const { + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = sparseJacobian(); + + // Convert boost tuples to Eigen triplets + vector> triplets; + triplets.reserve(entries.size()); + size_t rows = 0, cols = 0; + for (const auto& e : entries) { + size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); + triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); + rows = std::max(rows, temp_rows); + cols = std::max(cols, temp_cols); + } + + // ...and make a sparse matrix with it. + using SpMat = Eigen::SparseMatrix; + SpMat Ab(rows + 1, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + + // Solve A*x = b using sparse QR from Eigen + Eigen::SparseQR> qr(Ab.block(0, 0, rows+1, cols)); + Eigen::VectorXd x = qr.solve(Ab.col(cols)); + + return VectorValues(x, getKeyDimMap()); + } } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 2b9e8e6753..ca7631ac11 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -365,6 +365,9 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + /// solve Ax = b using SparseQR from Eigen + VectorValues eigenSparseQR() const; + /// @} private: From 2c91751578cbc9f78172601449b692651848f0ba Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sun, 4 Aug 2019 16:10:21 -0400 Subject: [PATCH 11/91] if params.isEigen(), can eigenSparseQR() to solve Ax=b --- gtsam/nonlinear/NonlinearOptimizer.cpp | 43 +------------------------- 1 file changed, 1 insertion(+), 42 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 2651135069..282a3c7b66 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -169,48 +169,7 @@ VectorValues NonlinearOptimizer::solve( "in LM solver ..."); } } else if (params.isEigen()) { - // TODO(Mandy): make this a GFG method ? - - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = gfg.sparseJacobian(); - - // Convert boost tuples to Eigen triplets - // TODO(Mandy): Levenberg-MQ prior has zeros in RHS - vector> triplets; - entries.reserve(triplets.size()); - for (const auto& e : entries) { - triplets.emplace_back(e.get<0>(), e.get<1>(), e.get<2>()); - cout << e.get<0>() << ", " << e.get<1>()<< ", " << e.get<2>() << endl; - } - - // ...and make a sparse matrix with it. - const auto& lastTriplet = triplets.back(); - const size_t rows = lastTriplet.row() + 1, cols = lastTriplet.col(); - using SpMat = Eigen::SparseMatrix; - SpMat Ab(rows, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - cout << Ab << endl; - - // Solve A*x = b using sparse QR from Eigen - Eigen::SparseQR> qr(Ab); - Eigen::VectorXd x = qr.solve(Ab.col(cols)); - cout << x << endl; - - // First find dimensions of each variable - std::map dims; - for (const auto& factor : gfg) { - if (!static_cast(factor)) - continue; - - for (GaussianFactor::const_iterator key = factor->begin(); - key != factor->end(); ++key) { - dims[*key] = factor->getDim(key); - } - } - delta = VectorValues(x, dims); - - // TODO(Mandy): make delta + delta = gfg.eigenSparseQR(); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From ebf3c2520c3dc22244ea0c3a8b0bdd8dd2c6a382 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Tue, 6 Aug 2019 11:23:52 -0400 Subject: [PATCH 12/91] rename eigenQR method --- gtsam/linear/GaussianFactorGraph.cpp | 64 +++++++++++++--------------- gtsam/linear/GaussianFactorGraph.h | 6 ++- 2 files changed, 33 insertions(+), 37 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 3591999f05..5f8a7a736f 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -322,6 +322,35 @@ namespace gtsam { return VectorValues(solution, scatter); } + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::optimizeEigenQR() const { + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = sparseJacobian(); + + // Convert boost tuples to Eigen triplets + vector> triplets; + triplets.reserve(entries.size()); + size_t rows = 0, cols = 0; + for (const auto& e : entries) { + size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); + triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); + rows = std::max(rows, temp_rows); + cols = std::max(cols, temp_cols); + } + + // ...and make a sparse matrix with it. + using SpMat = Eigen::SparseMatrix; + SpMat Ab(rows + 1, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + + // Solve A*x = b using sparse QR from Eigen + Eigen::SparseQR> qr(Ab.block(0, 0, rows+1, cols)); + Eigen::VectorXd x = qr.solve(Ab.col(cols)); + + return VectorValues(x, getKeyDimMap()); + } + /* ************************************************************************* */ namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { @@ -498,39 +527,4 @@ namespace gtsam { } return e; } - - /* ************************************************************************* */ - /** \deprecated */ - VectorValues GaussianFactorGraph::optimize(boost::none_t, - const Eliminate& function) const { - return optimize(function); - } - - VectorValues GaussianFactorGraph::eigenSparseQR() const { - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = sparseJacobian(); - - // Convert boost tuples to Eigen triplets - vector> triplets; - triplets.reserve(entries.size()); - size_t rows = 0, cols = 0; - for (const auto& e : entries) { - size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); - triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); - rows = std::max(rows, temp_rows); - cols = std::max(cols, temp_cols); - } - - // ...and make a sparse matrix with it. - using SpMat = Eigen::SparseMatrix; - SpMat Ab(rows + 1, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - - // Solve A*x = b using sparse QR from Eigen - Eigen::SparseQR> qr(Ab.block(0, 0, rows+1, cols)); - Eigen::VectorXd x = qr.solve(Ab.col(cols)); - - return VectorValues(x, getKeyDimMap()); - } } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ca7631ac11..504597ee3a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -297,6 +297,10 @@ namespace gtsam { */ VectorValues optimizeDensely() const; + + /// Optimize using Eigen's SparseQR factorization + VectorValues optimizeEigenQR() const; + /** * Compute the gradient of the energy function, * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, @@ -365,8 +369,6 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; - /// solve Ax = b using SparseQR from Eigen - VectorValues eigenSparseQR() const; /// @} From 9403181736f3b9d88d107b5b7ece285ae5016b43 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Tue, 6 Aug 2019 14:34:52 -0400 Subject: [PATCH 13/91] add unittest for eigen choleskey solver --- tests/testNonlinearOptimizer.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 45b116a412..370977030d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -174,10 +174,15 @@ TEST(NonlinearOptimizer, optimization_method) { Values actualMFChol = GaussNewtonOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); - // Test sparse Eigen solver - params.linearSolverType = GaussNewtonParams::EIGEN; - Values actualEigen = GaussNewtonOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0, fg.error(actualEigen), tol); + // Test sparse Eigen QR solver + params.linearSolverType = GaussNewtonParams::EIGEN_QR; + Values actualEigenQR = GaussNewtonOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actualEigenQR), tol); + + // Test sparse Eigen Cholesky solver + params.linearSolverType = GaussNewtonParams::EIGEN_CHOLESKY; + Values actualEigenCholesky = GaussNewtonOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actualEigenCholesky), tol); } /* ************************************************************************* */ From 77e222344662656a4e7353ac2e6ffbca10be9fce Mon Sep 17 00:00:00 2001 From: mxie32 Date: Tue, 6 Aug 2019 14:36:12 -0400 Subject: [PATCH 14/91] create separate files for eigen sparse solver --- gtsam/linear/EigenOptimizer.cpp | 77 +++++++++++++++++++++++++++++++++ gtsam/linear/EigenOptimizer.h | 33 ++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 gtsam/linear/EigenOptimizer.cpp create mode 100644 gtsam/linear/EigenOptimizer.h diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp new file mode 100644 index 0000000000..bac136a607 --- /dev/null +++ b/gtsam/linear/EigenOptimizer.cpp @@ -0,0 +1,77 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EigenOptimizer.cpp + * + * @brief optimizer linear factor graph using eigen solver as backend + * + * @date Aug 2019 + * @author Mandy Xie + */ + +#include +#include + +using namespace std; + +namespace gtsam { +using SpMat = Eigen::SparseMatrix; +/// obtain sparse matrix for eigen sparse solver +SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = gfg.sparseJacobian(); + + // Convert boost tuples to Eigen triplets + vector> triplets; + triplets.reserve(entries.size()); + size_t rows = 0, cols = 0; + for (const auto &e : entries) { + size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); + triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); + rows = std::max(rows, temp_rows); + cols = std::max(cols, temp_cols); + } + + // ...and make a sparse matrix with it. + using SpMat = Eigen::SparseMatrix; + SpMat Ab(rows + 1, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + return Ab; +} + +/* ************************************************************************* */ +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg) { + SpMat Ab = obtainSparseMatrix(gfg); + size_t rows = Ab.rows(); + size_t cols = Ab.cols(); + // Solve A*x = b using sparse QR from Eigen + Eigen::SparseQR> qr( + Ab.block(0, 0, rows, cols - 1)); + Eigen::VectorXd x = qr.solve(Ab.col(cols - 1)); + + return VectorValues(x, gfg.getKeyDimMap()); +} + +/* ************************************************************************* */ +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg) { + SpMat Ab = obtainSparseMatrix(gfg); + size_t rows = Ab.rows(); + size_t cols = Ab.cols(); + // Solve A*x = b using sparse QR from Eigen + Eigen::SimplicialLDLT ldlt(Ab.block(0, 0, rows, cols - 1)); + Eigen::VectorXd x = ldlt.solve(Ab.col(cols - 1)); + + return VectorValues(x, gfg.getKeyDimMap()); +} + +} //namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/EigenOptimizer.h new file mode 100644 index 0000000000..6ffb35f1ba --- /dev/null +++ b/gtsam/linear/EigenOptimizer.h @@ -0,0 +1,33 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EigenOptimizer.h + * + * @brief optimizer linear factor graph using eigen solver as backend + * + * @date Aug 2019 + * @author Mandy Xie + */ +#pragma once + +#include +#include +#include + +namespace gtsam { +/// Optimize using Eigen's SparseQR factorization +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg); + +/// Optimize using Eigen's SimplicailLDLT factorization +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg); + +} //namespace gtsam From 03a73a693440575249851de618c274bf838a5e3a Mon Sep 17 00:00:00 2001 From: mxie32 Date: Tue, 6 Aug 2019 14:38:23 -0400 Subject: [PATCH 15/91] add flag for eigen sparseQR and eigen cholesky, use this in nonlinear optimizer --- gtsam/linear/GaussianFactorGraph.cpp | 31 ---------------------- gtsam/linear/GaussianFactorGraph.h | 4 --- gtsam/nonlinear/NonlinearOptimizer.cpp | 9 ++++--- gtsam/nonlinear/NonlinearOptimizerParams.h | 11 +++++--- 4 files changed, 13 insertions(+), 42 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 5f8a7a736f..5c64bcf6ca 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -30,8 +30,6 @@ #include #include -#include - using namespace std; using namespace gtsam; @@ -322,35 +320,6 @@ namespace gtsam { return VectorValues(solution, scatter); } - /* ************************************************************************* */ - VectorValues GaussianFactorGraph::optimizeEigenQR() const { - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = sparseJacobian(); - - // Convert boost tuples to Eigen triplets - vector> triplets; - triplets.reserve(entries.size()); - size_t rows = 0, cols = 0; - for (const auto& e : entries) { - size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); - triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); - rows = std::max(rows, temp_rows); - cols = std::max(cols, temp_cols); - } - - // ...and make a sparse matrix with it. - using SpMat = Eigen::SparseMatrix; - SpMat Ab(rows + 1, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - - // Solve A*x = b using sparse QR from Eigen - Eigen::SparseQR> qr(Ab.block(0, 0, rows+1, cols)); - Eigen::VectorXd x = qr.solve(Ab.col(cols)); - - return VectorValues(x, getKeyDimMap()); - } - /* ************************************************************************* */ namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 504597ee3a..6fae5eef35 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -297,10 +297,6 @@ namespace gtsam { */ VectorValues optimizeDensely() const; - - /// Optimize using Eigen's SparseQR factorization - VectorValues optimizeEigenQR() const; - /** * Compute the gradient of the energy function, * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$, diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 282a3c7b66..6e031741f7 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -24,11 +24,10 @@ #include #include #include +#include #include -#include - #include #include @@ -168,8 +167,10 @@ VectorValues NonlinearOptimizer::solve( "NonlinearOptimizer::solve: special cg parameter type is not handled " "in LM solver ..."); } - } else if (params.isEigen()) { - delta = gfg.eigenSparseQR(); + } else if (params.isEigenQR()) { + delta = optimizeEigenQR(gfg); + } else if (params.isEigenCholesky()) { + delta = optimizeEigenCholesky(gfg); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index ef425e5db8..96eb9308f1 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -79,7 +79,8 @@ class GTSAM_EXPORT NonlinearOptimizerParams { SEQUENTIAL_QR, Iterative, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ - EIGEN, + EIGEN_QR, + EIGEN_CHOLESKY, }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer @@ -104,8 +105,12 @@ class GTSAM_EXPORT NonlinearOptimizerParams { return (linearSolverType == Iterative); } - inline bool isEigen() const { - return (linearSolverType == EIGEN); + inline bool isEigenQR() const { + return (linearSolverType == EIGEN_QR); + } + + inline bool isEigenCholesky() const { + return (linearSolverType == EIGEN_CHOLESKY); } GaussianFactorGraph::Eliminate getEliminationFunction() const { From 65be4d7a0388f775ba1cbff1eb980fa4130492b8 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Thu, 8 Aug 2019 20:58:06 -0400 Subject: [PATCH 16/91] add ordering type argument --- gtsam/linear/EigenOptimizer.cpp | 51 +++++++++++++++++++++++---------- gtsam/linear/EigenOptimizer.h | 9 ++++-- 2 files changed, 42 insertions(+), 18 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index bac136a607..ff37bc61c0 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -49,29 +49,50 @@ SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { return Ab; } -/* ************************************************************************* */ -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg) { - SpMat Ab = obtainSparseMatrix(gfg); +template +Eigen::VectorXd solve_x(const SpMat &Ab) { size_t rows = Ab.rows(); size_t cols = Ab.cols(); - // Solve A*x = b using sparse QR from Eigen - Eigen::SparseQR> qr( - Ab.block(0, 0, rows, cols - 1)); - Eigen::VectorXd x = qr.solve(Ab.col(cols - 1)); - - return VectorValues(x, gfg.getKeyDimMap()); + EigenSolverType solver(Ab.block(0, 0, rows, cols - 1)); + return solver.solve(Ab.col(cols - 1)); } /* ************************************************************************* */ -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg) { +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, + const Ordering::OrderingType &ordering) { SpMat Ab = obtainSparseMatrix(gfg); - size_t rows = Ab.rows(); - size_t cols = Ab.cols(); // Solve A*x = b using sparse QR from Eigen - Eigen::SimplicialLDLT ldlt(Ab.block(0, 0, rows, cols - 1)); - Eigen::VectorXd x = ldlt.solve(Ab.col(cols - 1)); + Eigen::VectorXd x; + switch (ordering) { + case Ordering::COLAMD: + x = solve_x>>(Ab); + case Ordering::NATURAL: + x = solve_x>>(Ab); + default: + x = solve_x>>(Ab); + } + return VectorValues(x, gfg.getKeyDimMap()); +} +/* ************************************************************************* + */ +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, + const Ordering::OrderingType &ordering) { + SpMat Ab = obtainSparseMatrix(gfg); + // Solve A*x = b using sparse QR from Eigen + Eigen::VectorXd x; + switch (ordering) { + case Ordering::COLAMD: + x = solve_x>>(Ab); + case Ordering::NATURAL: + x = solve_x>>(Ab); + default: + x = solve_x>>(Ab); + } return VectorValues(x, gfg.getKeyDimMap()); } -} //namespace gtsam \ No newline at end of file +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/EigenOptimizer.h index 6ffb35f1ba..19af0651a7 100644 --- a/gtsam/linear/EigenOptimizer.h +++ b/gtsam/linear/EigenOptimizer.h @@ -19,15 +19,18 @@ */ #pragma once +#include #include #include #include namespace gtsam { /// Optimize using Eigen's SparseQR factorization -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg); +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, + const Ordering::OrderingType &orderingType); /// Optimize using Eigen's SimplicailLDLT factorization -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg); +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, + const Ordering::OrderingType &orderingType); -} //namespace gtsam +} // namespace gtsam From c41bad3566cc738b8e0ca3100a7b4a80cc4f2437 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Thu, 8 Aug 2019 20:58:36 -0400 Subject: [PATCH 17/91] use param.orderingType when call eigenOptimizer --- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 6e031741f7..cbca6bab15 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -168,9 +168,9 @@ VectorValues NonlinearOptimizer::solve( "in LM solver ..."); } } else if (params.isEigenQR()) { - delta = optimizeEigenQR(gfg); + delta = optimizeEigenQR(gfg, params.orderingType); } else if (params.isEigenCholesky()) { - delta = optimizeEigenCholesky(gfg); + delta = optimizeEigenCholesky(gfg, params.orderingType); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From 14ab6e5fda84864738cacb4d01ec2fc9bd3d1c9e Mon Sep 17 00:00:00 2001 From: mxie32 Date: Fri, 9 Aug 2019 10:44:25 -0400 Subject: [PATCH 18/91] add metis ordering, and use string as argument type --- gtsam/linear/EigenOptimizer.cpp | 45 +++++++++++++++----------- gtsam/linear/EigenOptimizer.h | 6 ++-- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 +-- 3 files changed, 31 insertions(+), 24 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index ff37bc61c0..bd07ca4283 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -19,6 +19,7 @@ */ #include +#include #include using namespace std; @@ -59,17 +60,18 @@ Eigen::VectorXd solve_x(const SpMat &Ab) { /* ************************************************************************* */ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const Ordering::OrderingType &ordering) { + const std::string &orderingType) { SpMat Ab = obtainSparseMatrix(gfg); // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; - switch (ordering) { - case Ordering::COLAMD: - x = solve_x>>(Ab); - case Ordering::NATURAL: - x = solve_x>>(Ab); - default: - x = solve_x>>(Ab); + if (orderingType == "AMD") { + x = solve_x>>(Ab); + } else if (orderingType == "COLAMD") { + x = solve_x>>(Ab); + } else if (orderingType == "NATURAL") { + x = solve_x>>(Ab); + } else if (orderingType == "METIS") { + x = solve_x>>(Ab); } return VectorValues(x, gfg.getKeyDimMap()); } @@ -77,20 +79,25 @@ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, /* ************************************************************************* */ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const Ordering::OrderingType &ordering) { + const std::string &orderingType) { SpMat Ab = obtainSparseMatrix(gfg); // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; - switch (ordering) { - case Ordering::COLAMD: - x = solve_x>>(Ab); - case Ordering::NATURAL: - x = solve_x>>(Ab); - default: - x = solve_x>>(Ab); + if (orderingType == "AMD") { + x = solve_x< + Eigen::SimplicialLDLT>>( + Ab); + } else if (orderingType == "COLAMD") { + x = solve_x< + Eigen::SimplicialLDLT>>( + Ab); + } else if (orderingType == "NATURAL") { + x = solve_x>>(Ab); + } else if (orderingType == "METIS") { + x = solve_x< + Eigen::SimplicialLDLT>>( + Ab); } return VectorValues(x, gfg.getKeyDimMap()); } diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/EigenOptimizer.h index 19af0651a7..fbc29d6de0 100644 --- a/gtsam/linear/EigenOptimizer.h +++ b/gtsam/linear/EigenOptimizer.h @@ -19,18 +19,18 @@ */ #pragma once -#include #include #include #include +#include namespace gtsam { /// Optimize using Eigen's SparseQR factorization VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const Ordering::OrderingType &orderingType); + const std::string &orderingType); /// Optimize using Eigen's SimplicailLDLT factorization VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const Ordering::OrderingType &orderingType); + const std::string &orderingType); } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index cbca6bab15..9163f85e4f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -168,9 +168,9 @@ VectorValues NonlinearOptimizer::solve( "in LM solver ..."); } } else if (params.isEigenQR()) { - delta = optimizeEigenQR(gfg, params.orderingType); + delta = optimizeEigenQR(gfg, params.getOrderingType()); } else if (params.isEigenCholesky()) { - delta = optimizeEigenCholesky(gfg, params.orderingType); + delta = optimizeEigenCholesky(gfg, params.getOrderingType()); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From fedd538ce03e6e8bc476bf0621be7e8f3ed6e944 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Fri, 9 Aug 2019 16:21:27 -0400 Subject: [PATCH 19/91] add solveCholesky method --- gtsam/linear/EigenOptimizer.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index bd07ca4283..a12122ac24 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -51,7 +51,7 @@ SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { } template -Eigen::VectorXd solve_x(const SpMat &Ab) { +Eigen::VectorXd solveQR(const SpMat &Ab) { size_t rows = Ab.rows(); size_t cols = Ab.cols(); EigenSolverType solver(Ab.block(0, 0, rows, cols - 1)); @@ -65,17 +65,28 @@ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; if (orderingType == "AMD") { - x = solve_x>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "COLAMD") { - x = solve_x>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "NATURAL") { - x = solve_x>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "METIS") { - x = solve_x>>(Ab); + x = solveQR>>(Ab); } return VectorValues(x, gfg.getKeyDimMap()); } +template +Eigen::VectorXd solveCholesky(const SpMat &Ab) { + size_t rows = Ab.rows(); + size_t cols = Ab.cols(); + auto A = Ab.block(0, 0, rows, cols - 1); + auto At = A.transpose(); + auto b = Ab.col(cols - 1); + EigenSolverType solver(At*A); + return solver.solve(At*b); +} + /* ************************************************************************* */ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, @@ -84,18 +95,18 @@ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; if (orderingType == "AMD") { - x = solve_x< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "COLAMD") { - x = solve_x< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "NATURAL") { - x = solve_x>>(Ab); } else if (orderingType == "METIS") { - x = solve_x< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } From ee2f03e54c7d220a5586b9d2bdd3d68b2324bdd0 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Fri, 9 Aug 2019 16:21:50 -0400 Subject: [PATCH 20/91] change GN to LM in unittest --- tests/testNonlinearOptimizer.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 370977030d..3d1e43c38c 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -162,26 +162,26 @@ TEST(NonlinearOptimizer, optimization_method) { c0.insert(X(1), x0); // Below we solve with different backend linear solver choices - GaussNewtonParams params; + LevenbergMarquardtParams params; // Multifrontal QR, will be parallel if TBB installed - params.linearSolverType = GaussNewtonParams::MULTIFRONTAL_QR; - Values actualMFQR = GaussNewtonOptimizer(fg, c0, params).optimize(); + params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; + Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); // Multifrontal Cholesky (more sensitive to conditioning, but faster) - params.linearSolverType = GaussNewtonParams::MULTIFRONTAL_CHOLESKY; - Values actualMFChol = GaussNewtonOptimizer(fg, c0, params).optimize(); + params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; + Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); // Test sparse Eigen QR solver - params.linearSolverType = GaussNewtonParams::EIGEN_QR; - Values actualEigenQR = GaussNewtonOptimizer(fg, c0, params).optimize(); + params.linearSolverType = LevenbergMarquardtParams::EIGEN_QR; + Values actualEigenQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenQR), tol); // Test sparse Eigen Cholesky solver - params.linearSolverType = GaussNewtonParams::EIGEN_CHOLESKY; - Values actualEigenCholesky = GaussNewtonOptimizer(fg, c0, params).optimize(); + params.linearSolverType = LevenbergMarquardtParams::EIGEN_CHOLESKY; + Values actualEigenCholesky = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenCholesky), tol); } From 1b6d9f076e9427108e800d533e6a8b2726bc869b Mon Sep 17 00:00:00 2001 From: mxie32 Date: Fri, 9 Aug 2019 16:41:05 -0400 Subject: [PATCH 21/91] add tictoc to eigen solver --- gtsam/linear/EigenOptimizer.cpp | 40 ++++++++++++++++++++++----------- 1 file changed, 27 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index a12122ac24..f53a7d5cfc 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -18,6 +18,7 @@ * @author Mandy Xie */ +#include #include #include #include @@ -28,6 +29,7 @@ namespace gtsam { using SpMat = Eigen::SparseMatrix; /// obtain sparse matrix for eigen sparse solver SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { + gttic_(EigenOptimizer_obtainSparseMatrix); // Get sparse entries of Jacobian [A|b] augmented with RHS b. auto entries = gfg.sparseJacobian(); @@ -51,7 +53,8 @@ SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { } template -Eigen::VectorXd solveQR(const SpMat &Ab) { +Eigen::VectorXd createQR(const SpMat &Ab) { + gttic_(EigenOptimizer_createQR); size_t rows = Ab.rows(); size_t cols = Ab.cols(); EigenSolverType solver(Ab.block(0, 0, rows, cols - 1)); @@ -61,52 +64,63 @@ Eigen::VectorXd solveQR(const SpMat &Ab) { /* ************************************************************************* */ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, const std::string &orderingType) { + gttic_(EigenOptimizer_optimizeEigenQR); SpMat Ab = obtainSparseMatrix(gfg); // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; if (orderingType == "AMD") { - x = solveQR>>(Ab); + gttic_(EigenOptimizer_optimizeEigenQR_AMD); + x = createQR>>(Ab); } else if (orderingType == "COLAMD") { - x = solveQR>>(Ab); + gttic_(EigenOptimizer_optimizeEigenQR_COLAMD); + x = createQR>>(Ab); } else if (orderingType == "NATURAL") { - x = solveQR>>(Ab); + gttic_(EigenOptimizer_optimizeEigenQR_NATURAL); + x = createQR>>(Ab); } else if (orderingType == "METIS") { - x = solveQR>>(Ab); + gttic_(EigenOptimizer_optimizeEigenQR_METIS); + x = createQR>>(Ab); } return VectorValues(x, gfg.getKeyDimMap()); } template -Eigen::VectorXd solveCholesky(const SpMat &Ab) { +Eigen::VectorXd createCholesky(const SpMat &Ab) { + gttic_(EigenOptimizer_createCholesky); size_t rows = Ab.rows(); size_t cols = Ab.cols(); auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); auto b = Ab.col(cols - 1); - EigenSolverType solver(At*A); - return solver.solve(At*b); + EigenSolverType solver(At * A); + return solver.solve(At * b); } /* ************************************************************************* */ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, const std::string &orderingType) { + gttic_(EigenOptimizer_optimizeEigenCholesky); SpMat Ab = obtainSparseMatrix(gfg); // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; if (orderingType == "AMD") { - x = solveCholesky< + gttic_(EigenOptimizer_optimizeEigenCholesky_AMD); + x = createCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "COLAMD") { - x = solveCholesky< + gttic_(EigenOptimizer_optimizeEigenCholesky_COLAMD); + x = createCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "NATURAL") { - x = solveCholesky>>(Ab); + gttic_(EigenOptimizer_optimizeEigenCholesky_NATURAL); + x = createCholesky>>(Ab); } else if (orderingType == "METIS") { - x = solveCholesky< + gttic_(EigenOptimizer_optimizeEigenCholesky_METIS); + x = createCholesky< Eigen::SimplicialLDLT>>( Ab); } From 5a7634a6179bbd0e99b8a101986bf6b227b94b0d Mon Sep 17 00:00:00 2001 From: mxie32 Date: Sun, 11 Aug 2019 12:15:53 -0400 Subject: [PATCH 22/91] profile --- gtsam/linear/EigenOptimizer.cpp | 36 +++++++++++++++------------- gtsam/linear/GaussianFactorGraph.cpp | 1 + 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index f53a7d5cfc..b5009052ad 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -37,24 +37,30 @@ SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { vector> triplets; triplets.reserve(entries.size()); size_t rows = 0, cols = 0; + + gttic_(EigenOptimizer_obtainSparseMatrix_for_loop); for (const auto &e : entries) { size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); rows = std::max(rows, temp_rows); cols = std::max(cols, temp_cols); } - + gttoc_(EigenOptimizer_obtainSparseMatrix_for_loop); // ...and make a sparse matrix with it. using SpMat = Eigen::SparseMatrix; SpMat Ab(rows + 1, cols + 1); + gttic_(EigenOptimizer_obtainSparseMatrix_setFromTriplets); Ab.setFromTriplets(triplets.begin(), triplets.end()); + gttoc_(EigenOptimizer_obtainSparseMatrix_setFromTriplets); + gttic_(EigenOptimizer_obtainSparseMatrix_makeCompressed); Ab.makeCompressed(); + gttoc_(EigenOptimizer_obtainSparseMatrix_makeCompressed); return Ab; } template -Eigen::VectorXd createQR(const SpMat &Ab) { - gttic_(EigenOptimizer_createQR); +Eigen::VectorXd solveQR(const SpMat &Ab) { + gttic_(EigenOptimizer_solveQR); size_t rows = Ab.rows(); size_t cols = Ab.cols(); EigenSolverType solver(Ab.block(0, 0, rows, cols - 1)); @@ -70,23 +76,23 @@ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, Eigen::VectorXd x; if (orderingType == "AMD") { gttic_(EigenOptimizer_optimizeEigenQR_AMD); - x = createQR>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "COLAMD") { gttic_(EigenOptimizer_optimizeEigenQR_COLAMD); - x = createQR>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "NATURAL") { gttic_(EigenOptimizer_optimizeEigenQR_NATURAL); - x = createQR>>(Ab); + x = solveQR>>(Ab); } else if (orderingType == "METIS") { gttic_(EigenOptimizer_optimizeEigenQR_METIS); - x = createQR>>(Ab); + x = solveQR>>(Ab); } return VectorValues(x, gfg.getKeyDimMap()); } template -Eigen::VectorXd createCholesky(const SpMat &Ab) { - gttic_(EigenOptimizer_createCholesky); +Eigen::VectorXd solveCholesky(const SpMat &Ab) { + gttic_(EigenOptimizer_solveCholesky); size_t rows = Ab.rows(); size_t cols = Ab.cols(); auto A = Ab.block(0, 0, rows, cols - 1); @@ -105,22 +111,18 @@ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, // Solve A*x = b using sparse QR from Eigen Eigen::VectorXd x; if (orderingType == "AMD") { - gttic_(EigenOptimizer_optimizeEigenCholesky_AMD); - x = createCholesky< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "COLAMD") { - gttic_(EigenOptimizer_optimizeEigenCholesky_COLAMD); - x = createCholesky< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } else if (orderingType == "NATURAL") { - gttic_(EigenOptimizer_optimizeEigenCholesky_NATURAL); - x = createCholesky>>(Ab); } else if (orderingType == "METIS") { - gttic_(EigenOptimizer_optimizeEigenCholesky_METIS); - x = createCholesky< + x = solveCholesky< Eigen::SimplicialLDLT>>( Ab); } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 5c64bcf6ca..e2f46d7fea 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -101,6 +101,7 @@ namespace gtsam { /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian() const { + gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable std::map dims; for (const sharedFactor& factor : *this) { From 362a4f285fc7ed3817d5b91c612753fdda073e06 Mon Sep 17 00:00:00 2001 From: mxie32 Date: Mon, 12 Aug 2019 13:47:47 -0400 Subject: [PATCH 23/91] add optional ordering to sparseJacobian --- gtsam/linear/GaussianFactorGraph.cpp | 28 ++++-- gtsam/linear/GaussianFactorGraph.h | 5 +- .../linear/tests/testGaussianFactorGraph.cpp | 98 +++++++++++++------ 3 files changed, 90 insertions(+), 41 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index e2f46d7fea..cd7ed7642c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -100,7 +100,9 @@ namespace gtsam { } /* ************************************************************************* */ - vector > GaussianFactorGraph::sparseJacobian() const { + vector > + GaussianFactorGraph::sparseJacobian( + boost::optional ordering) const { gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable std::map dims; @@ -108,18 +110,24 @@ namespace gtsam { if (!static_cast(factor)) continue; - for (GaussianFactor::const_iterator key = factor->begin(); - key != factor->end(); ++key) { - dims[*key] = factor->getDim(key); + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); } } // Compute first scalar column of each variable size_t currentColIndex = 0; std::map columnIndices; - for (const auto& keyValue : dims) { - columnIndices[keyValue.first] = currentColIndex; - currentColIndex += keyValue.second; + if (ordering) { + for (const auto key : *ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + } else { + for (const auto& keyValue : dims) { + columnIndices[keyValue.first] = currentColIndex; + currentColIndex += keyValue.second; + } } // Iterate over all factors, adding sparse scalar entries @@ -174,11 +182,11 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparseJacobian_() const { - + Matrix GaussianFactorGraph::sparseJacobian_( + boost::optional optionalOrdering) const { // call sparseJacobian typedef boost::tuple triplet; - vector result = sparseJacobian(); + vector result = sparseJacobian(optionalOrdering); // translate to base 1 matrix size_t nzmax = result.size(); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6fae5eef35..0e20eb3096 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -185,14 +185,15 @@ namespace gtsam { * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. * The standard deviations are baked into A and b */ - std::vector > sparseJacobian() const; + std::vector > sparseJacobian( + boost::optional optionalOrdering = boost::none) const; /** * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * The standard deviations are baked into A and b */ - Matrix sparseJacobian_() const; + Matrix sparseJacobian_(boost::optional optionalOrdering = boost::none) const; /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index b480871ec2..7861197aad 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -11,7 +11,7 @@ /** * @file testGaussianFactorGraph.cpp - * @brief Unit tests for Gaussian (i.e., Linear) Factor Graph + * @brief Unit tests for Gaussian (i.e, Linear) Factor Graph * @author Christian Potthast * @author Frank Dellaert * @author Luca Carlone @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -57,10 +58,12 @@ TEST(GaussianFactorGraph, initialization) { // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 Matrix expectedIJS = - (Matrix(3, 22) << 1., 2., 1., 2., 3., 4., 3., 4., 3., 4., 5., 6., 5., 6., 5., 6., 7., 8., 7., - 8., 7., 8., 1., 2., 7., 7., 1., 2., 3., 4., 7., 7., 1., 2., 5., 6., 7., 7., 3., 4., 5., 6., - 7., 7., 10., 10., -1., -1., -10., -10., 10., 10., 2., -1., -5., -5., 5., 5., 0., 1., -5., - -5., 5., 5., -1., 1.5).finished(); + (Matrix(3, 21) << 1, 2, 1, 2, 3, 4, 3, 4, 3, 4, 5, 6, 5, 6, 6, 7, 8, 7, 8, + 7, 8, // + 1, 2, 7, 7, 1, 2, 3, 4, 7, 7, 1, 2, 5, 6, 7, 3, 4, 5, 6, 7, 7, // + 10, 10, -1, -1, -10, -10, 10, 10, 2, -1, -5, -5, 5, 5, 1, -5, -5, 5, 5, + -1, 1.5) + .finished(); Matrix actualIJS = fg.sparseJacobian_(); EQUALITY(expectedIJS, actualIJS); } @@ -68,49 +71,86 @@ TEST(GaussianFactorGraph, initialization) { /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: - // x1 x2 x3 x4 x5 b + // x1 x2 x3 x4 x3 b // 1 2 3 0 0 4 // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.).finished(), Vector2(4., 8.), model); - gfg.add(0, (Matrix(2, 3) << 9., 10., 0., 0., 0., 0.).finished(), 1, - (Matrix(2, 2) << 11., 12., 14., 15.).finished(), Vector2(13., 16.), model); + const Key x3 = 0, y2 = 1; + // const Symbol x3('x', 5), y2('p', 3); + gfg.add(x3, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(), Vector2(4, 8), model); + gfg.add(x3, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(), y2, + (Matrix(2, 2) << 11, 12, 14, 15.).finished(), Vector2(13, 16), model); - // Check the triplets size... auto entries = gfg.sparseJacobian(); + // Check the triplets size... EXPECT_LONGS_EQUAL(16, entries.size()); // Check version for MATLAB - NOTE that we transpose this! Matrix expectedT = (Matrix(16, 3) << - 1., 1., 2., - 1., 2., 4., - 1., 3., 6., - 2., 1.,10., - 2., 2.,12., - 2., 3.,14., - 1., 6., 8., - 2., 6.,16., - 3., 1.,18., - 3., 2.,20., - 3., 4.,22., - 3., 5.,24., - 4., 4.,28., - 4., 5.,30., - 3., 6.,26., - 4., 6.,32.).finished(); + 1, 1, 2, + 1, 2, 4, + 1, 3, 6, + 2, 1,10, + 2, 2,12, + 2, 3,14, + 1, 6, 8, + 2, 6,16, + 3, 1,18, + 3, 2,20, + 3, 4,22, + 3, 5,24, + 4, 4,28, + 4, 5,30, + 3, 6,26, + 4, 6,32).finished(); Matrix expectedMatlab = expectedT.transpose(); Matrix matlab = gfg.sparseJacobian_(); EXPECT(assert_equal(expectedMatlab, matlab)); + + // Call sparseJacobian with optional ordering... + auto ordering = Ordering(list_of(y2)(x3)); + entries = gfg.sparseJacobian(ordering); + // Check the triplets size... + EXPECT_LONGS_EQUAL(16, entries.size()); + + // Create factor graph: + // x4 x3 x1 x2 x3 b + // 0 0 1 2 3 4 + // 0 0 5 6 7 8 + // 11 12 9 10 0 13 + // 14 15 0 0 0 16 + // Check version for MATLAB - NOTE that we transpose this! + expectedT = (Matrix(16, 3) << + 1, 3, 2, + 1, 4, 4, + 1, 5, 6, + 2, 3,10, + 2, 4,12, + 2, 5,14, + 1, 6, 8, + 2, 6,16, + 3, 3,18, + 3, 4,20, + 3, 1,22, + 3, 2,24, + 4, 1,28, + 4, 2,30, + 3, 6,26, + 4, 6,32).finished(); + expectedMatlab = expectedT.transpose(); + matlab = gfg.sparseJacobian_(ordering); + + EXPECT(assert_equal(expectedMatlab, matlab)); } /* ************************************************************************* */ TEST(GaussianFactorGraph, matrices) { // Create factor graph: - // x1 x2 x3 x4 x5 b + // x1 x2 x3 x4 x3 b // 1 2 3 0 0 4 // 5 6 7 0 0 8 // 9 10 0 11 12 13 @@ -122,8 +162,8 @@ TEST(GaussianFactorGraph, matrices) { GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, A00, Vector2(4., 8.), model); - gfg.add(0, A10, 1, A11, Vector2(13., 16.), model); + gfg.add(0, A00, Vector2(4, 8), model); + gfg.add(0, A10, 1, A11, Vector2(13, 16), model); Matrix Ab(4, 6); Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16; From d9f1fe9daae7d99a96551000110d979fc40ee92a Mon Sep 17 00:00:00 2001 From: mxie32 Date: Mon, 12 Aug 2019 21:23:41 -0400 Subject: [PATCH 24/91] use ordering in obtainSparseMatrix, and add tic for A transpose --- gtsam/linear/EigenOptimizer.cpp | 121 +++++++++++++++----------------- gtsam/linear/EigenOptimizer.h | 4 ++ 2 files changed, 59 insertions(+), 66 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index b5009052ad..26a5a2061c 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -28,105 +28,94 @@ using namespace std; namespace gtsam { using SpMat = Eigen::SparseMatrix; /// obtain sparse matrix for eigen sparse solver -SpMat obtainSparseMatrix(const GaussianFactorGraph &gfg) { +std::pair obtainSparseMatrix( + const GaussianFactorGraph &gfg, const Ordering &ordering) { gttic_(EigenOptimizer_obtainSparseMatrix); // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = gfg.sparseJacobian(); - + auto entries = gfg.sparseJacobian(ordering); // Convert boost tuples to Eigen triplets vector> triplets; triplets.reserve(entries.size()); size_t rows = 0, cols = 0; - - gttic_(EigenOptimizer_obtainSparseMatrix_for_loop); for (const auto &e : entries) { size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); rows = std::max(rows, temp_rows); cols = std::max(cols, temp_cols); } - gttoc_(EigenOptimizer_obtainSparseMatrix_for_loop); // ...and make a sparse matrix with it. - using SpMat = Eigen::SparseMatrix; SpMat Ab(rows + 1, cols + 1); - gttic_(EigenOptimizer_obtainSparseMatrix_setFromTriplets); Ab.setFromTriplets(triplets.begin(), triplets.end()); - gttoc_(EigenOptimizer_obtainSparseMatrix_setFromTriplets); - gttic_(EigenOptimizer_obtainSparseMatrix_makeCompressed); Ab.makeCompressed(); - gttoc_(EigenOptimizer_obtainSparseMatrix_makeCompressed); - return Ab; -} - -template -Eigen::VectorXd solveQR(const SpMat &Ab) { - gttic_(EigenOptimizer_solveQR); - size_t rows = Ab.rows(); - size_t cols = Ab.cols(); - EigenSolverType solver(Ab.block(0, 0, rows, cols - 1)); - return solver.solve(Ab.col(cols - 1)); + return make_pair(Ab.block(0, 0, rows + 1, cols), + Ab.col(cols)); } /* ************************************************************************* */ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const std::string &orderingType) { + const Ordering &ordering) { gttic_(EigenOptimizer_optimizeEigenQR); - SpMat Ab = obtainSparseMatrix(gfg); + auto Ab_pair = obtainSparseMatrix(gfg, ordering); // Solve A*x = b using sparse QR from Eigen - Eigen::VectorXd x; - if (orderingType == "AMD") { - gttic_(EigenOptimizer_optimizeEigenQR_AMD); - x = solveQR>>(Ab); - } else if (orderingType == "COLAMD") { - gttic_(EigenOptimizer_optimizeEigenQR_COLAMD); - x = solveQR>>(Ab); - } else if (orderingType == "NATURAL") { - gttic_(EigenOptimizer_optimizeEigenQR_NATURAL); - x = solveQR>>(Ab); + gttic_(EigenOptimizer_optimizeEigenQR_create_solver); + Eigen::SparseQR> solver(Ab_pair.first); + gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); + gttic_(EigenOptimizer_optimizeEigenQR_solve); + Eigen::VectorXd x = solver.solve(Ab_pair.second); + gttoc_(EigenOptimizer_optimizeEigenQR_solve); + return VectorValues(x, gfg.getKeyDimMap()); +} + +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, + const std::string &orderingType) { + if (orderingType == "COLAMD") { + return optimizeEigenQR(gfg, Ordering::Colamd(gfg)); } else if (orderingType == "METIS") { - gttic_(EigenOptimizer_optimizeEigenQR_METIS); - x = solveQR>>(Ab); + return optimizeEigenQR(gfg, Ordering::Metis(gfg)); + } else if (orderingType == "NATURAL") { + return optimizeEigenQR(gfg, Ordering::Natural(gfg)); + } else { + std::cout + << "No applicable ordering method, use COLAMD ordering by default !!" + << std::endl; + return optimizeEigenQR(gfg, Ordering::Colamd(gfg)); } - return VectorValues(x, gfg.getKeyDimMap()); } -template -Eigen::VectorXd solveCholesky(const SpMat &Ab) { - gttic_(EigenOptimizer_solveCholesky); - size_t rows = Ab.rows(); - size_t cols = Ab.cols(); - auto A = Ab.block(0, 0, rows, cols - 1); +/* *************************************************************************/ +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, + const Ordering &ordering) { + gttic_(EigenOptimizer_optimizeEigenCholesky); + auto Ab_pair = obtainSparseMatrix(gfg, ordering); + auto A = Ab_pair.first; + gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose); auto At = A.transpose(); - auto b = Ab.col(cols - 1); - EigenSolverType solver(At * A); - return solver.solve(At * b); + gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose); + gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); + // Solve A*x = b using sparse QR from Eigen + Eigen::SimplicialLDLT> + solver(At * A); + gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); + gttic_(EigenOptimizer_optimizeEigenCholesky_solve); + Eigen::VectorXd x = solver.solve(At * Ab_pair.second); + gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); + return VectorValues(x, gfg.getKeyDimMap()); } -/* ************************************************************************* - */ VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, const std::string &orderingType) { - gttic_(EigenOptimizer_optimizeEigenCholesky); - SpMat Ab = obtainSparseMatrix(gfg); - // Solve A*x = b using sparse QR from Eigen - Eigen::VectorXd x; - if (orderingType == "AMD") { - x = solveCholesky< - Eigen::SimplicialLDLT>>( - Ab); - } else if (orderingType == "COLAMD") { - x = solveCholesky< - Eigen::SimplicialLDLT>>( - Ab); - } else if (orderingType == "NATURAL") { - x = solveCholesky>>(Ab); + if (orderingType == "COLAMD") { + return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg)); } else if (orderingType == "METIS") { - x = solveCholesky< - Eigen::SimplicialLDLT>>( - Ab); + return optimizeEigenCholesky(gfg, Ordering::Metis(gfg)); + } else if (orderingType == "NATURAL") { + return optimizeEigenCholesky(gfg, Ordering::Natural(gfg)); + } else { + std::cout + << "No applicable ordering method, use COLAMD ordering by default !!" + << std::endl; + return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg)); } - return VectorValues(x, gfg.getKeyDimMap()); } } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/EigenOptimizer.h index fbc29d6de0..5522ef5b52 100644 --- a/gtsam/linear/EigenOptimizer.h +++ b/gtsam/linear/EigenOptimizer.h @@ -26,10 +26,14 @@ namespace gtsam { /// Optimize using Eigen's SparseQR factorization +VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, + const Ordering &ordering); VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, const std::string &orderingType); /// Optimize using Eigen's SimplicailLDLT factorization +VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, + const Ordering &ordering); VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, const std::string &orderingType); From 4e40d1ba08fb83844b6c5d7cdedbbdb5f44ada0b Mon Sep 17 00:00:00 2001 From: mxie32 Date: Wed, 14 Aug 2019 12:34:24 -0400 Subject: [PATCH 25/91] add unit test for eigenOptimizer --- gtsam/linear/tests/testEigenOptimizer.cpp | 75 +++++++++++++++++++++++ 1 file changed, 75 insertions(+) create mode 100644 gtsam/linear/tests/testEigenOptimizer.cpp diff --git a/gtsam/linear/tests/testEigenOptimizer.cpp b/gtsam/linear/tests/testEigenOptimizer.cpp new file mode 100644 index 0000000000..0bd7e16fbe --- /dev/null +++ b/gtsam/linear/tests/testEigenOptimizer.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testEigenOptimizer.cpp + * @brief Unit tests for Eigen optimizer + * @author Mandy Xie + * @author Frank Dellaert + **/ + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +/// Factor graph with 2 2D factors on 3 2D variables +static GaussianFactorGraph createSimpleGaussianFactorGraph() { + GaussianFactorGraph fg; + Key x1 = 2, x2 = 0, l1 = 1; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + return fg; +} + +/* ************************************************************************* */ +TEST(EigenOptimizer, optimizeEigenQR) { + GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + + VectorValues expected; + expected.insert(2, Vector2(-0.1, -0.1)); + expected.insert(0, Vector2(0.1, -0.2)); + expected.insert(1, Vector2(-0.1, 0.1)); + + VectorValues actual = optimizeEigenQR(A, "COLAMD"); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(EigenOptimizer, optimizeEigenCholesky) { + GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + + VectorValues expected; + expected.insert(2, Vector2(-0.1, -0.1)); + expected.insert(0, Vector2(0.1, -0.2)); + expected.insert(1, Vector2(-0.1, 0.1)); + + VectorValues actual = optimizeEigenCholesky(A, "COLAMD"); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 351e962ac84e778af4f03b6432a68de6a850548e Mon Sep 17 00:00:00 2001 From: mxie32 Date: Wed, 14 Aug 2019 22:48:36 -0400 Subject: [PATCH 26/91] use optional ordering in obtainSparseMatrix --- gtsam/linear/EigenOptimizer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp index 26a5a2061c..b6a5162179 100644 --- a/gtsam/linear/EigenOptimizer.cpp +++ b/gtsam/linear/EigenOptimizer.cpp @@ -29,7 +29,8 @@ namespace gtsam { using SpMat = Eigen::SparseMatrix; /// obtain sparse matrix for eigen sparse solver std::pair obtainSparseMatrix( - const GaussianFactorGraph &gfg, const Ordering &ordering) { + const GaussianFactorGraph &gfg, + boost::optional ordering = boost::none) { gttic_(EigenOptimizer_obtainSparseMatrix); // Get sparse entries of Jacobian [A|b] augmented with RHS b. auto entries = gfg.sparseJacobian(ordering); @@ -67,7 +68,7 @@ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, } VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const std::string &orderingType) { + const std::string &orderingType) { if (orderingType == "COLAMD") { return optimizeEigenQR(gfg, Ordering::Colamd(gfg)); } else if (orderingType == "METIS") { From b5582254457fa76a20b6cceced0b21b7f7c5efb4 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 09:45:48 -0400 Subject: [PATCH 27/91] Initialize Refactoring --- gtsam/geometry/triangulation.cpp | 2 +- gtsam/linear/LinearOptimizer.cpp | 24 ++++++++++++++++++ gtsam/linear/LinearOptimizer.h | 29 ++++++++++++++++++++++ gtsam/linear/LinearOptimizerParams.cpp | 5 ++++ gtsam/linear/LinearOptimizerParams.h | 29 ++++++++++++++++++++++ gtsam/nonlinear/NonlinearOptimizerParams.h | 13 ++-------- 6 files changed, 90 insertions(+), 12 deletions(-) create mode 100644 gtsam/linear/LinearOptimizer.cpp create mode 100644 gtsam/linear/LinearOptimizer.h create mode 100644 gtsam/linear/LinearOptimizerParams.cpp create mode 100644 gtsam/linear/LinearOptimizerParams.h diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a5d2e04cd4..a3cf68208c 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -82,7 +82,7 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, params.absoluteErrorTol = 1.0; params.verbosityLM = LevenbergMarquardtParams::SILENT; params.verbosity = NonlinearOptimizerParams::SILENT; - params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = MULTIFRONTAL_CHOLESKY; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); diff --git a/gtsam/linear/LinearOptimizer.cpp b/gtsam/linear/LinearOptimizer.cpp new file mode 100644 index 0000000000..f4bbdfe860 --- /dev/null +++ b/gtsam/linear/LinearOptimizer.cpp @@ -0,0 +1,24 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearOptimizer.cpp + * @brief Common Interface for Linear Optimizers + * @author Fan Jiang + */ + +#include "LinearOptimizer.h" + +LinearOptimizer::LinearOptimizer() {} + +LinearOptimizer::LinearOptimizer(gtsam::NonlinearOptimizerParams param) { + +} diff --git a/gtsam/linear/LinearOptimizer.h b/gtsam/linear/LinearOptimizer.h new file mode 100644 index 0000000000..713918183f --- /dev/null +++ b/gtsam/linear/LinearOptimizer.h @@ -0,0 +1,29 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearOptimizer.h + * @brief Common Interface for Linear Optimizers + * @author Fan Jiang + */ + +#pragma once + +#include "gtsam/nonlinear/NonlinearOptimizerParams.h" + +class LinearOptimizer { +public: + LinearOptimizer(); + LinearOptimizer(gtsam::NonlinearOptimizerParams param); + + LinearOptimizer(LinearOptimizer&) = delete; +}; + diff --git a/gtsam/linear/LinearOptimizerParams.cpp b/gtsam/linear/LinearOptimizerParams.cpp new file mode 100644 index 0000000000..e16eaf0844 --- /dev/null +++ b/gtsam/linear/LinearOptimizerParams.cpp @@ -0,0 +1,5 @@ +// +// Created by fan on 9/8/19. +// + +#include "LinearOptimizerParams.h" diff --git a/gtsam/linear/LinearOptimizerParams.h b/gtsam/linear/LinearOptimizerParams.h new file mode 100644 index 0000000000..b781d140ce --- /dev/null +++ b/gtsam/linear/LinearOptimizerParams.h @@ -0,0 +1,29 @@ +// +// Created by fan on 9/8/19. +// + +#ifndef GTSAM_LINEAROPTIMIZERPARAMS_H +#define GTSAM_LINEAROPTIMIZERPARAMS_H + +namespace gtsam { + + /** See NonlinearOptimizerParams::linearSolverType */ + typedef enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + Iterative, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + EIGEN_QR, + EIGEN_CHOLESKY, + } LinearSolverType; +} + + +class LinearOptimizerParams { + +}; + + +#endif //GTSAM_LINEAROPTIMIZERPARAMS_H diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 96eb9308f1..175abe7d28 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -71,17 +72,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - /** See NonlinearOptimizerParams::linearSolverType */ - enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - EIGEN_QR, - EIGEN_CHOLESKY, - }; + using LinearSolverType = gtsam::LinearSolverType; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) From 10ac7f783c526b907e883ba4a4f3dcfa5768faeb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 10:35:26 -0400 Subject: [PATCH 28/91] Rename LinearOptimizer to LinearSolver --- gtsam/linear/LinearOptimizer.h | 29 ------------- gtsam/linear/LinearOptimizerParams.h | 29 ------------- .../{LinearOptimizer.cpp => LinearSolver.cpp} | 14 +++++-- gtsam/linear/LinearSolver.h | 42 +++++++++++++++++++ ...mizerParams.cpp => LinearSolverParams.cpp} | 2 +- gtsam/linear/LinearSolverParams.h | 38 +++++++++++++++++ gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- tests/testNonlinearOptimizer.cpp | 8 ++-- tests/testPCGSolver.cpp | 6 +-- 9 files changed, 99 insertions(+), 71 deletions(-) delete mode 100644 gtsam/linear/LinearOptimizer.h delete mode 100644 gtsam/linear/LinearOptimizerParams.h rename gtsam/linear/{LinearOptimizer.cpp => LinearSolver.cpp} (67%) create mode 100644 gtsam/linear/LinearSolver.h rename gtsam/linear/{LinearOptimizerParams.cpp => LinearSolverParams.cpp} (50%) create mode 100644 gtsam/linear/LinearSolverParams.h diff --git a/gtsam/linear/LinearOptimizer.h b/gtsam/linear/LinearOptimizer.h deleted file mode 100644 index 713918183f..0000000000 --- a/gtsam/linear/LinearOptimizer.h +++ /dev/null @@ -1,29 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file LinearOptimizer.h - * @brief Common Interface for Linear Optimizers - * @author Fan Jiang - */ - -#pragma once - -#include "gtsam/nonlinear/NonlinearOptimizerParams.h" - -class LinearOptimizer { -public: - LinearOptimizer(); - LinearOptimizer(gtsam::NonlinearOptimizerParams param); - - LinearOptimizer(LinearOptimizer&) = delete; -}; - diff --git a/gtsam/linear/LinearOptimizerParams.h b/gtsam/linear/LinearOptimizerParams.h deleted file mode 100644 index b781d140ce..0000000000 --- a/gtsam/linear/LinearOptimizerParams.h +++ /dev/null @@ -1,29 +0,0 @@ -// -// Created by fan on 9/8/19. -// - -#ifndef GTSAM_LINEAROPTIMIZERPARAMS_H -#define GTSAM_LINEAROPTIMIZERPARAMS_H - -namespace gtsam { - - /** See NonlinearOptimizerParams::linearSolverType */ - typedef enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - EIGEN_QR, - EIGEN_CHOLESKY, - } LinearSolverType; -} - - -class LinearOptimizerParams { - -}; - - -#endif //GTSAM_LINEAROPTIMIZERPARAMS_H diff --git a/gtsam/linear/LinearOptimizer.cpp b/gtsam/linear/LinearSolver.cpp similarity index 67% rename from gtsam/linear/LinearOptimizer.cpp rename to gtsam/linear/LinearSolver.cpp index f4bbdfe860..fba60381e0 100644 --- a/gtsam/linear/LinearOptimizer.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -15,10 +15,16 @@ * @author Fan Jiang */ -#include "LinearOptimizer.h" +#include "LinearSolver.h" -LinearOptimizer::LinearOptimizer() {} +namespace gtsam { -LinearOptimizer::LinearOptimizer(gtsam::NonlinearOptimizerParams param) { + std::unique_ptr LinearSolver::fromNonlinearParams(gtsam::NonlinearOptimizerParams nlparams) { + return std::unique_ptr(); + } -} + LinearSolver::LinearSolver() { + + } + +} \ No newline at end of file diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h new file mode 100644 index 0000000000..856cba2da5 --- /dev/null +++ b/gtsam/linear/LinearSolver.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearSolver.h + * @brief Common Interface for Linear Solvers + * @author Fan Jiang + */ + +#pragma once + +#include +#include + +namespace gtsam { + class LinearSolver { + public: + LinearSolver(LinearSolver &) = delete; + + gtsam::LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer + + virtual bool isIterative() = 0; + + virtual bool isSequential() = 0; + + static std::unique_ptr fromNonlinearParams(gtsam::NonlinearOptimizerParams nlparams); + + virtual VectorValues solve(const GaussianFactorGraph &gfg) = 0; + + protected: + LinearSolver(); + }; + +} \ No newline at end of file diff --git a/gtsam/linear/LinearOptimizerParams.cpp b/gtsam/linear/LinearSolverParams.cpp similarity index 50% rename from gtsam/linear/LinearOptimizerParams.cpp rename to gtsam/linear/LinearSolverParams.cpp index e16eaf0844..faf370b304 100644 --- a/gtsam/linear/LinearOptimizerParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -2,4 +2,4 @@ // Created by fan on 9/8/19. // -#include "LinearOptimizerParams.h" +#include "LinearSolverParams.h" diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h new file mode 100644 index 0000000000..abcde0939c --- /dev/null +++ b/gtsam/linear/LinearSolverParams.h @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearSolver.h + * @brief Common Interface for Linear Solvers + * @author Fan Jiang + */ + +#pragma once + +namespace gtsam { + + /** See NonlinearOptimizerParams::linearSolverType */ + typedef enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + Iterative, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + EIGEN_QR, + EIGEN_CHOLESKY, + } LinearSolverType; +} + + +class LinearSolverParams { + +}; \ No newline at end of file diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 175abe7d28..b34a6ef8c1 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 3d1e43c38c..243c2fc4cd 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -165,22 +165,22 @@ TEST(NonlinearOptimizer, optimization_method) { LevenbergMarquardtParams params; // Multifrontal QR, will be parallel if TBB installed - params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; + params.linearSolverType = MULTIFRONTAL_QR; Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); // Multifrontal Cholesky (more sensitive to conditioning, but faster) - params.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = MULTIFRONTAL_CHOLESKY; Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); // Test sparse Eigen QR solver - params.linearSolverType = LevenbergMarquardtParams::EIGEN_QR; + params.linearSolverType = EIGEN_QR; Values actualEigenQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenQR), tol); // Test sparse Eigen Cholesky solver - params.linearSolverType = LevenbergMarquardtParams::EIGEN_CHOLESKY; + params.linearSolverType = EIGEN_CHOLESKY; Values actualEigenCholesky = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenCholesky), tol); } diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 66b022c822..ba05e51fe2 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -129,7 +129,7 @@ TEST( GaussianFactorGraphSystem, multiply_getb) TEST( PCGSolver, dummy ) { LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + paramsPCG.linearSolverType = Iterative; PCGSolverParameters::shared_ptr pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); paramsPCG.iterativeParams = pcg; @@ -150,7 +150,7 @@ TEST( PCGSolver, dummy ) TEST( PCGSolver, blockjacobi ) { LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + paramsPCG.linearSolverType = Iterative; PCGSolverParameters::shared_ptr pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); paramsPCG.iterativeParams = pcg; @@ -171,7 +171,7 @@ TEST( PCGSolver, blockjacobi ) TEST( PCGSolver, subgraph ) { LevenbergMarquardtParams paramsPCG; - paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + paramsPCG.linearSolverType = Iterative; PCGSolverParameters::shared_ptr pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); paramsPCG.iterativeParams = pcg; From 6ae60a8a1641fd76cc9fd9ceedeff87b143615a9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 12:44:57 -0400 Subject: [PATCH 29/91] Fix typo --- gtsam/linear/EigenOptimizer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/EigenOptimizer.h index 5522ef5b52..f2affdeead 100644 --- a/gtsam/linear/EigenOptimizer.h +++ b/gtsam/linear/EigenOptimizer.h @@ -31,7 +31,7 @@ VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, const std::string &orderingType); -/// Optimize using Eigen's SimplicailLDLT factorization +/// Optimize using Eigen's SimplicialLDLT factorization VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, const Ordering &ordering); VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, From 0aa0d9b64ea34c04e85035604bc2caec978e097b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 16:48:54 -0400 Subject: [PATCH 30/91] WIP with EigenSolver --- gtsam/linear/EigenOptimizer.cpp | 122 ------------------ gtsam/linear/LinearSolver.cpp | 21 ++- gtsam/linear/LinearSolver.h | 17 ++- gtsam/linear/SparseEigenSolver.cpp | 99 ++++++++++++++ .../{EigenOptimizer.h => SparseEigenSolver.h} | 25 ++++ gtsam/linear/tests/testEigenOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 10 +- 7 files changed, 159 insertions(+), 137 deletions(-) delete mode 100644 gtsam/linear/EigenOptimizer.cpp create mode 100644 gtsam/linear/SparseEigenSolver.cpp rename gtsam/linear/{EigenOptimizer.h => SparseEigenSolver.h} (74%) diff --git a/gtsam/linear/EigenOptimizer.cpp b/gtsam/linear/EigenOptimizer.cpp deleted file mode 100644 index b6a5162179..0000000000 --- a/gtsam/linear/EigenOptimizer.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file EigenOptimizer.cpp - * - * @brief optimizer linear factor graph using eigen solver as backend - * - * @date Aug 2019 - * @author Mandy Xie - */ - -#include -#include -#include -#include - -using namespace std; - -namespace gtsam { -using SpMat = Eigen::SparseMatrix; -/// obtain sparse matrix for eigen sparse solver -std::pair obtainSparseMatrix( - const GaussianFactorGraph &gfg, - boost::optional ordering = boost::none) { - gttic_(EigenOptimizer_obtainSparseMatrix); - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = gfg.sparseJacobian(ordering); - // Convert boost tuples to Eigen triplets - vector> triplets; - triplets.reserve(entries.size()); - size_t rows = 0, cols = 0; - for (const auto &e : entries) { - size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); - triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); - rows = std::max(rows, temp_rows); - cols = std::max(cols, temp_cols); - } - // ...and make a sparse matrix with it. - SpMat Ab(rows + 1, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - return make_pair(Ab.block(0, 0, rows + 1, cols), - Ab.col(cols)); -} - -/* ************************************************************************* */ -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const Ordering &ordering) { - gttic_(EigenOptimizer_optimizeEigenQR); - auto Ab_pair = obtainSparseMatrix(gfg, ordering); - // Solve A*x = b using sparse QR from Eigen - gttic_(EigenOptimizer_optimizeEigenQR_create_solver); - Eigen::SparseQR> solver(Ab_pair.first); - gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); - gttic_(EigenOptimizer_optimizeEigenQR_solve); - Eigen::VectorXd x = solver.solve(Ab_pair.second); - gttoc_(EigenOptimizer_optimizeEigenQR_solve); - return VectorValues(x, gfg.getKeyDimMap()); -} - -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const std::string &orderingType) { - if (orderingType == "COLAMD") { - return optimizeEigenQR(gfg, Ordering::Colamd(gfg)); - } else if (orderingType == "METIS") { - return optimizeEigenQR(gfg, Ordering::Metis(gfg)); - } else if (orderingType == "NATURAL") { - return optimizeEigenQR(gfg, Ordering::Natural(gfg)); - } else { - std::cout - << "No applicable ordering method, use COLAMD ordering by default !!" - << std::endl; - return optimizeEigenQR(gfg, Ordering::Colamd(gfg)); - } -} - -/* *************************************************************************/ -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const Ordering &ordering) { - gttic_(EigenOptimizer_optimizeEigenCholesky); - auto Ab_pair = obtainSparseMatrix(gfg, ordering); - auto A = Ab_pair.first; - gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose); - auto At = A.transpose(); - gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose); - gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); - // Solve A*x = b using sparse QR from Eigen - Eigen::SimplicialLDLT> - solver(At * A); - gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); - gttic_(EigenOptimizer_optimizeEigenCholesky_solve); - Eigen::VectorXd x = solver.solve(At * Ab_pair.second); - gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); - return VectorValues(x, gfg.getKeyDimMap()); -} - -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const std::string &orderingType) { - if (orderingType == "COLAMD") { - return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg)); - } else if (orderingType == "METIS") { - return optimizeEigenCholesky(gfg, Ordering::Metis(gfg)); - } else if (orderingType == "NATURAL") { - return optimizeEigenCholesky(gfg, Ordering::Natural(gfg)); - } else { - std::cout - << "No applicable ordering method, use COLAMD ordering by default !!" - << std::endl; - return optimizeEigenCholesky(gfg, Ordering::Colamd(gfg)); - } -} - -} // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index fba60381e0..783fffe127 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -15,16 +15,27 @@ * @author Fan Jiang */ -#include "LinearSolver.h" +#include +#include namespace gtsam { - std::unique_ptr LinearSolver::fromNonlinearParams(gtsam::NonlinearOptimizerParams nlparams) { - return std::unique_ptr(); - } + std::shared_ptr LinearSolver::fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams) { + + boost::optional optionalOrdering; + if (nlparams.ordering) optionalOrdering.reset(*nlparams.ordering); - LinearSolver::LinearSolver() { + if (nlparams.isEigenQR()) { + return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR)); + } else if (nlparams.isEigenCholesky()) { + return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY)); + } + + throw std::runtime_error( + "Invalid parameters passed"); } + LinearSolver::LinearSolver() = default; + } \ No newline at end of file diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 856cba2da5..2433c19413 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { class LinearSolver { @@ -27,16 +28,24 @@ namespace gtsam { gtsam::LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer - virtual bool isIterative() = 0; + virtual bool isIterative() { + return false; + }; - virtual bool isSequential() = 0; + virtual bool isSequential() { + return false; + }; - static std::unique_ptr fromNonlinearParams(gtsam::NonlinearOptimizerParams nlparams); + static std::shared_ptr fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams); - virtual VectorValues solve(const GaussianFactorGraph &gfg) = 0; + virtual VectorValues solve(const GaussianFactorGraph &gfg, const Ordering &ordering) { + throw std::runtime_error( + "BUG_CHECK: Calling solve of the base class!"); + }; protected: LinearSolver(); + virtual ~LinearSolver() = default; }; } \ No newline at end of file diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp new file mode 100644 index 0000000000..3e59953ee9 --- /dev/null +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EigenOptimizer.cpp + * + * @brief optimizer linear factor graph using eigen solver as backend + * + * @date Aug 2019 + * @author Mandy Xie + */ + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { +using SpMat = Eigen::SparseMatrix; +/// obtain sparse matrix for eigen sparse solver +std::pair obtainSparseMatrix( + const GaussianFactorGraph &gfg, + boost::optional ordering = boost::none) { + gttic_(EigenOptimizer_obtainSparseMatrix); + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = gfg.sparseJacobian(ordering); + // Convert boost tuples to Eigen triplets + vector> triplets; + triplets.reserve(entries.size()); + size_t rows = 0, cols = 0; + for (const auto &e : entries) { + size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); + triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); + rows = std::max(rows, temp_rows); + cols = std::max(cols, temp_cols); + } + // ...and make a sparse matrix with it. + SpMat Ab(rows + 1, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + return make_pair(Ab.block(0, 0, rows + 1, cols), + Ab.col(cols)); +} + + bool SparseEigenSolver::isIterative() { + return false; + } + + bool SparseEigenSolver::isSequential() { + return false; + } + + VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg, const Ordering &ordering) { + if (solverType == QR) { + gttic_(EigenOptimizer_optimizeEigenQR); + auto Ab_pair = obtainSparseMatrix(gfg, ordering); + // Solve A*x = b using sparse QR from Eigen + gttic_(EigenOptimizer_optimizeEigenQR_create_solver); + Eigen::SparseQR> solver(Ab_pair.first); + gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); + gttic_(EigenOptimizer_optimizeEigenQR_solve); + Eigen::VectorXd x = solver.solve(Ab_pair.second); + gttoc_(EigenOptimizer_optimizeEigenQR_solve); + return VectorValues(x, gfg.getKeyDimMap()); + } else if (solverType == CHOLESKY) { + gttic_(EigenOptimizer_optimizeEigenCholesky); + auto Ab_pair = obtainSparseMatrix(gfg, ordering); + auto A = Ab_pair.first; + gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose); + auto At = A.transpose(); + gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose); + gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); + // Solve A*x = b using sparse QR from Eigen + Eigen::SimplicialLDLT> + solver(At * A); + gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); + gttic_(EigenOptimizer_optimizeEigenCholesky_solve); + Eigen::VectorXd x = solver.solve(At * Ab_pair.second); + gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); + return VectorValues(x, gfg.getKeyDimMap()); + } + + return VectorValues(); // Should throw? + } + + SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type) { + solverType = type; + } +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/EigenOptimizer.h b/gtsam/linear/SparseEigenSolver.h similarity index 74% rename from gtsam/linear/EigenOptimizer.h rename to gtsam/linear/SparseEigenSolver.h index f2affdeead..e1c38411bc 100644 --- a/gtsam/linear/EigenOptimizer.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -21,10 +21,35 @@ #include #include +#include #include #include namespace gtsam { + +class SparseEigenSolver : public gtsam::LinearSolver { + public: + + typedef enum { + QR, + CHOLESKY + } SparseEigenSolverType; + + + explicit SparseEigenSolver(SparseEigenSolverType type); + + bool isIterative() override; + + bool isSequential() override; + + VectorValues solve(const GaussianFactorGraph &gfg, const Ordering &ordering) override; + + protected: + + SparseEigenSolverType solverType = QR; + + }; + /// Optimize using Eigen's SparseQR factorization VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, const Ordering &ordering); diff --git a/gtsam/linear/tests/testEigenOptimizer.cpp b/gtsam/linear/tests/testEigenOptimizer.cpp index 0bd7e16fbe..62adb5d78b 100644 --- a/gtsam/linear/tests/testEigenOptimizer.cpp +++ b/gtsam/linear/tests/testEigenOptimizer.cpp @@ -16,7 +16,7 @@ * @author Frank Dellaert **/ -#include +#include #include #include diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 9163f85e4f..660fc2f542 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -24,7 +24,8 @@ #include #include #include -#include +#include +#include #include @@ -167,10 +168,9 @@ VectorValues NonlinearOptimizer::solve( "NonlinearOptimizer::solve: special cg parameter type is not handled " "in LM solver ..."); } - } else if (params.isEigenQR()) { - delta = optimizeEigenQR(gfg, params.getOrderingType()); - } else if (params.isEigenCholesky()) { - delta = optimizeEigenCholesky(gfg, params.getOrderingType()); + } else if (params.isEigenQR() || params.isEigenCholesky()) { + LinearSolver solver = LinearSolver::fromNonlinearParams(params); + delta = solver.solve(gfg, *optionalOrdering); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From 08c487f76512b66000cbf4584b5ce3cb80364d9b Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 19:24:57 -0400 Subject: [PATCH 31/91] Make the tests happy again! --- gtsam/linear/SparseEigenSolver.h | 13 ------------- gtsam/linear/tests/testEigenOptimizer.cpp | 6 ++++-- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 ++-- 3 files changed, 6 insertions(+), 17 deletions(-) diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index e1c38411bc..7edd2b738b 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -49,17 +49,4 @@ class SparseEigenSolver : public gtsam::LinearSolver { SparseEigenSolverType solverType = QR; }; - -/// Optimize using Eigen's SparseQR factorization -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const Ordering &ordering); -VectorValues optimizeEigenQR(const GaussianFactorGraph &gfg, - const std::string &orderingType); - -/// Optimize using Eigen's SimplicialLDLT factorization -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const Ordering &ordering); -VectorValues optimizeEigenCholesky(const GaussianFactorGraph &gfg, - const std::string &orderingType); - } // namespace gtsam diff --git a/gtsam/linear/tests/testEigenOptimizer.cpp b/gtsam/linear/tests/testEigenOptimizer.cpp index 62adb5d78b..9ac38c2677 100644 --- a/gtsam/linear/tests/testEigenOptimizer.cpp +++ b/gtsam/linear/tests/testEigenOptimizer.cpp @@ -50,7 +50,8 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - VectorValues actual = optimizeEigenQR(A, "COLAMD"); + auto solver = new SparseEigenSolver(SparseEigenSolver::QR); + VectorValues actual = solver->solve(A, Ordering::Colamd(A)); EXPECT(assert_equal(expected, actual)); } @@ -63,7 +64,8 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - VectorValues actual = optimizeEigenCholesky(A, "COLAMD"); + auto solver = new SparseEigenSolver(SparseEigenSolver::CHOLESKY); + VectorValues actual = solver->solve(A, Ordering::Colamd(A)); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 660fc2f542..c8bc2cb4d9 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -169,8 +169,8 @@ VectorValues NonlinearOptimizer::solve( "in LM solver ..."); } } else if (params.isEigenQR() || params.isEigenCholesky()) { - LinearSolver solver = LinearSolver::fromNonlinearParams(params); - delta = solver.solve(gfg, *optionalOrdering); + auto solver = LinearSolver::fromNonlinearParams(params); + delta = solver->solve(gfg, *optionalOrdering); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From af4c944f5ac5c1b7d8359522f9396aaa6f49d4ca Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 19:39:33 -0400 Subject: [PATCH 32/91] Fix docs and cleanup --- gtsam/linear/LinearSolver.h | 14 +++++- gtsam/linear/SparseEigenSolver.cpp | 72 +++++++++++++++++++----------- gtsam/linear/SparseEigenSolver.h | 7 ++- 3 files changed, 62 insertions(+), 31 deletions(-) diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 2433c19413..e5743c483a 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -22,11 +22,14 @@ #include namespace gtsam { + + /** Common Interface Class for all linear solvers */ class LinearSolver { public: LinearSolver(LinearSolver &) = delete; - gtsam::LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer + // TODO: Remove this and use trait functions instead? + gtsam::LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of this instance virtual bool isIterative() { return false; @@ -36,15 +39,22 @@ namespace gtsam { return false; }; + /// + /** + * Factor method for generating a LinearSolver from legacy NonlinearOptimizerParams + * @param nonlinear optimizer parameters + * @return pointer to a LinearSolver object + */ static std::shared_ptr fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams); virtual VectorValues solve(const GaussianFactorGraph &gfg, const Ordering &ordering) { throw std::runtime_error( - "BUG_CHECK: Calling solve of the base class!"); + "BUG_CHECK: Calling solve of the base class!"); }; protected: LinearSolver(); + virtual ~LinearSolver() = default; }; diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 3e59953ee9..514a1f7b15 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -10,12 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file EigenOptimizer.cpp + * @file SparseEigenSolver.cpp * - * @brief optimizer linear factor graph using eigen solver as backend + * @brief Eigen SparseSolver based linear solver backend for GTSAM * * @date Aug 2019 * @author Mandy Xie + * @author Fan Jiang + * @author Frank Dellaert */ #include @@ -26,31 +28,40 @@ using namespace std; namespace gtsam { -using SpMat = Eigen::SparseMatrix; -/// obtain sparse matrix for eigen sparse solver -std::pair obtainSparseMatrix( - const GaussianFactorGraph &gfg, - boost::optional ordering = boost::none) { - gttic_(EigenOptimizer_obtainSparseMatrix); - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = gfg.sparseJacobian(ordering); - // Convert boost tuples to Eigen triplets - vector> triplets; - triplets.reserve(entries.size()); - size_t rows = 0, cols = 0; - for (const auto &e : entries) { - size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); - triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); - rows = std::max(rows, temp_rows); - cols = std::max(cols, temp_cols); + + using SpMat = Eigen::SparseMatrix; + + /// obtain sparse matrix for eigen sparse solver + std::pair obtainSparseMatrix( + const GaussianFactorGraph &gfg, + boost::optional ordering = boost::none) { + + gttic_(EigenOptimizer_obtainSparseMatrix); + + // Get sparse entries of Jacobian [A|b] augmented with RHS b. + auto entries = gfg.sparseJacobian(ordering); + + // Convert boost tuples to Eigen triplets + vector> triplets; + triplets.reserve(entries.size()); + size_t rows = 0, cols = 0; + for (const auto &e : entries) { + size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); + triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); + rows = std::max(rows, temp_rows); + cols = std::max(cols, temp_cols); + } + + // ...and make a sparse matrix with it. + SpMat Ab(rows + 1, cols + 1); + Ab.setFromTriplets(triplets.begin(), triplets.end()); + Ab.makeCompressed(); + + gttoc_(EigenOptimizer_obtainSparseMatrix); + + return make_pair(Ab.block(0, 0, rows + 1, cols), + Ab.col(cols)); } - // ...and make a sparse matrix with it. - SpMat Ab(rows + 1, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - return make_pair(Ab.block(0, 0, rows + 1, cols), - Ab.col(cols)); -} bool SparseEigenSolver::isIterative() { return false; @@ -64,29 +75,36 @@ std::pair obtainSparseMatrix( if (solverType == QR) { gttic_(EigenOptimizer_optimizeEigenQR); auto Ab_pair = obtainSparseMatrix(gfg, ordering); + // Solve A*x = b using sparse QR from Eigen gttic_(EigenOptimizer_optimizeEigenQR_create_solver); Eigen::SparseQR> solver(Ab_pair.first); gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); + gttic_(EigenOptimizer_optimizeEigenQR_solve); Eigen::VectorXd x = solver.solve(Ab_pair.second); gttoc_(EigenOptimizer_optimizeEigenQR_solve); + return VectorValues(x, gfg.getKeyDimMap()); } else if (solverType == CHOLESKY) { gttic_(EigenOptimizer_optimizeEigenCholesky); auto Ab_pair = obtainSparseMatrix(gfg, ordering); auto A = Ab_pair.first; + gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose); auto At = A.transpose(); gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose); + gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); // Solve A*x = b using sparse QR from Eigen Eigen::SimplicialLDLT> - solver(At * A); + solver(At * A); gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); + gttic_(EigenOptimizer_optimizeEigenCholesky_solve); Eigen::VectorXd x = solver.solve(At * Ab_pair.second); gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); + return VectorValues(x, gfg.getKeyDimMap()); } diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 7edd2b738b..6e3eef1622 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -10,13 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file EigenOptimizer.h + * @file SparseEigenSolver.h * - * @brief optimizer linear factor graph using eigen solver as backend + * @brief Eigen SparseSolver based linear solver backend for GTSAM * * @date Aug 2019 * @author Mandy Xie + * @author Fan Jiang + * @author Frank Dellaert */ + #pragma once #include From 3ef6112321c49fe5810a5437794c27188861e7fe Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 19:48:09 -0400 Subject: [PATCH 33/91] Make the examples happy again! --- examples/Pose2SLAMwSPCG.cpp | 2 +- examples/SFMExample_SmartFactorPCG.cpp | 2 +- examples/SolverComparer.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 9b459d7b83..272a92a6f0 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -69,7 +69,7 @@ int main(int argc, char** argv) { // We indicate that an iterative linear solver should be used. // In addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.linearSolverType = Iterative; parameters.iterativeParams = boost::make_shared(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index da7c5ba9b5..8d664324ac 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) { // In addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) LevenbergMarquardtParams parameters; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.linearSolverType = Iterative; parameters.absoluteErrorTol = 1e-10; parameters.relativeErrorTol = 1e-10; parameters.maxIterations = 500; diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 80ac08e030..7b296cee78 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -468,7 +468,7 @@ void runBatch() gttic_(Create_optimizer); GaussNewtonParams params; - params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = MULTIFRONTAL_CHOLESKY; GaussNewtonOptimizer optimizer(measurements, initial, params); gttoc_(Create_optimizer); double lastError; From e848d0af34bb45a4374744172d0f6ffbafcdeee9 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 9 Sep 2019 19:48:26 -0400 Subject: [PATCH 34/91] More indentation fixes --- gtsam/linear/LinearSolverParams.h | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index abcde0939c..58f886204c 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -19,17 +19,18 @@ namespace gtsam { - /** See NonlinearOptimizerParams::linearSolverType */ - typedef enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - EIGEN_QR, - EIGEN_CHOLESKY, - } LinearSolverType; + // TODO: Remove this enum + /** See NonlinearOptimizerParams::linearSolverType */ + typedef enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + Iterative, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + EIGEN_QR, + EIGEN_CHOLESKY, + } LinearSolverType; } From f3f299fb8bb40b55d3b18fb6fa9ca777e23bd06c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 10 Sep 2019 12:32:54 -0400 Subject: [PATCH 35/91] Order should be in the constructor of SparseEigenSolver --- gtsam/linear/LinearSolver.cpp | 4 ++-- gtsam/linear/LinearSolver.h | 2 +- gtsam/linear/SparseEigenSolver.cpp | 5 +++-- gtsam/linear/SparseEigenSolver.h | 5 +++-- gtsam/linear/tests/testEigenOptimizer.cpp | 8 ++++---- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 783fffe127..f5b60e9bda 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -26,9 +26,9 @@ namespace gtsam { if (nlparams.ordering) optionalOrdering.reset(*nlparams.ordering); if (nlparams.isEigenQR()) { - return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR)); + return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *optionalOrdering)); } else if (nlparams.isEigenCholesky()) { - return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY)); + return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *optionalOrdering)); } throw std::runtime_error( diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index e5743c483a..908726ca5d 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -47,7 +47,7 @@ namespace gtsam { */ static std::shared_ptr fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams); - virtual VectorValues solve(const GaussianFactorGraph &gfg, const Ordering &ordering) { + virtual VectorValues solve(const GaussianFactorGraph &gfg) { throw std::runtime_error( "BUG_CHECK: Calling solve of the base class!"); }; diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 514a1f7b15..bb5e806166 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -71,7 +71,7 @@ namespace gtsam { return false; } - VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg, const Ordering &ordering) { + VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) { if (solverType == QR) { gttic_(EigenOptimizer_optimizeEigenQR); auto Ab_pair = obtainSparseMatrix(gfg, ordering); @@ -111,7 +111,8 @@ namespace gtsam { return VectorValues(); // Should throw? } - SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type) { + SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) { solverType = type; + this->ordering = ordering; } } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 6e3eef1622..b62ae6961b 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -39,17 +39,18 @@ class SparseEigenSolver : public gtsam::LinearSolver { } SparseEigenSolverType; - explicit SparseEigenSolver(SparseEigenSolverType type); + explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering); bool isIterative() override; bool isSequential() override; - VectorValues solve(const GaussianFactorGraph &gfg, const Ordering &ordering) override; + VectorValues solve(const GaussianFactorGraph &gfg) override; protected: SparseEigenSolverType solverType = QR; + Ordering ordering; }; } // namespace gtsam diff --git a/gtsam/linear/tests/testEigenOptimizer.cpp b/gtsam/linear/tests/testEigenOptimizer.cpp index 9ac38c2677..69f9601c70 100644 --- a/gtsam/linear/tests/testEigenOptimizer.cpp +++ b/gtsam/linear/tests/testEigenOptimizer.cpp @@ -50,8 +50,8 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - auto solver = new SparseEigenSolver(SparseEigenSolver::QR); - VectorValues actual = solver->solve(A, Ordering::Colamd(A)); + auto solver = new SparseEigenSolver(SparseEigenSolver::QR, Ordering::Colamd(A)); + VectorValues actual = solver->solve(A); EXPECT(assert_equal(expected, actual)); } @@ -64,8 +64,8 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - auto solver = new SparseEigenSolver(SparseEigenSolver::CHOLESKY); - VectorValues actual = solver->solve(A, Ordering::Colamd(A)); + auto solver = new SparseEigenSolver(SparseEigenSolver::CHOLESKY, Ordering::Colamd(A)); + VectorValues actual = solver->solve(A); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index c8bc2cb4d9..18a8c37c8a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -170,7 +170,7 @@ VectorValues NonlinearOptimizer::solve( } } else if (params.isEigenQR() || params.isEigenCholesky()) { auto solver = LinearSolver::fromNonlinearParams(params); - delta = solver->solve(gfg, *optionalOrdering); + delta = solver->solve(gfg); } else { throw std::runtime_error( "NonlinearOptimizer::solve: Optimization parameter is invalid"); From 084a25b9e0d8501830a29379921f8a52d37d9409 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 11 Sep 2019 10:54:50 -0400 Subject: [PATCH 36/91] Added test, renamed testEigenOptimizer to testSparseEigenSolver --- gtsam/linear/LinearSolver.cpp | 4 +- gtsam/linear/SparseEigenSolver.h | 5 +- gtsam/linear/tests/testLinearSolver.cpp | 88 +++++++++++++++++++ ...ptimizer.cpp => testSparseEigenSolver.cpp} | 0 4 files changed, 94 insertions(+), 3 deletions(-) create mode 100644 gtsam/linear/tests/testLinearSolver.cpp rename gtsam/linear/tests/{testEigenOptimizer.cpp => testSparseEigenSolver.cpp} (100%) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index f5b60e9bda..5908249972 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file LinearOptimizer.cpp - * @brief Common Interface for Linear Optimizers + * @file LinearSolver.cpp + * @brief Common Interface for Linear Solvers * @author Fan Jiang */ diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index b62ae6961b..ea88403823 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -30,7 +30,10 @@ namespace gtsam { -class SparseEigenSolver : public gtsam::LinearSolver { + /** + * Eigen SparseSolver based Backend class + */ + class SparseEigenSolver : public gtsam::LinearSolver { public: typedef enum { diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp new file mode 100644 index 0000000000..81c682d424 --- /dev/null +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -0,0 +1,88 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLinearSolver.cpp + * @brief Tests for Common Interface for Linear Solvers + * @author Fan Jiang + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +/* ************************************************************************* */ +/// Factor graph with 2 2D factors on 3 2D variables +static GaussianFactorGraph createSimpleGaussianFactorGraph() { + GaussianFactorGraph fg; + Key x1 = 2, x2 = 0, l1 = 1; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] + fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + // odometry between x1 and x2: x2-x1=[0.2;-0.1] + fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); + // measurement between x1 and l1: l1-x1=[0.0;0.2] + fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); + // measurement between x2 and l1: l1-x2=[-0.2;0.3] + fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); + return fg; +} + +/* ************************************************************************* */ +TEST(EigenOptimizer, optimizeEigenQR) { + GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + + VectorValues expected; + expected.insert(2, Vector2(-0.1, -0.1)); + expected.insert(0, Vector2(0.1, -0.2)); + expected.insert(1, Vector2(-0.1, 0.1)); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::ERROR; + parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; + parameters.linearSolverType = EIGEN_QR; + parameters.ordering = Ordering::Colamd(A); + + auto solver = LinearSolver::fromNonlinearParams(parameters); + VectorValues actual = solver->solve(A); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +TEST(EigenOptimizer, optimizeEigenCholesky) { + GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + + VectorValues expected; + expected.insert(2, Vector2(-0.1, -0.1)); + expected.insert(0, Vector2(0.1, -0.2)); + expected.insert(1, Vector2(-0.1, 0.1)); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::ERROR; + parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; + parameters.linearSolverType = EIGEN_CHOLESKY; + parameters.ordering = Ordering::Colamd(A); + + auto solver = LinearSolver::fromNonlinearParams(parameters); + VectorValues actual = solver->solve(A); + EXPECT(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testEigenOptimizer.cpp b/gtsam/linear/tests/testSparseEigenSolver.cpp similarity index 100% rename from gtsam/linear/tests/testEigenOptimizer.cpp rename to gtsam/linear/tests/testSparseEigenSolver.cpp From 34eb36982ffe482278541991d517d776352345b2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 18 Nov 2019 10:52:29 -0500 Subject: [PATCH 37/91] Refactoring to disentangle linear and nonlinear --- gtsam/linear/LinearSolver.cpp | 21 ++++++++++---------- gtsam/linear/LinearSolver.h | 4 ++-- gtsam/linear/LinearSolverParams.h | 13 ++++++++---- gtsam/linear/SparseEigenSolver.h | 2 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 5 ++++- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 1 - gtsam/nonlinear/NonlinearOptimizerParams.h | 5 ++++- 7 files changed, 31 insertions(+), 20 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 5908249972..4f42fffbc8 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -20,22 +20,23 @@ namespace gtsam { - std::shared_ptr LinearSolver::fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams) { + LinearSolver::LinearSolver() = default; + + std::shared_ptr LinearSolver::fromLinearSolverParams(const LinearSolverParams ¶ms) { - boost::optional optionalOrdering; - if (nlparams.ordering) optionalOrdering.reset(*nlparams.ordering); + boost::optional optionalOrdering; + if (params.ordering) optionalOrdering.reset(*params.ordering); - if (nlparams.isEigenQR()) { - return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *optionalOrdering)); - } else if (nlparams.isEigenCholesky()) { - return std::shared_ptr(new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *optionalOrdering)); + if (params.solverType == EIGEN_QR) { + return std::shared_ptr( + new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *optionalOrdering)); + } else if (params.solverType == EIGEN_CHOLESKY) { + return std::shared_ptr( + new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *optionalOrdering)); } throw std::runtime_error( "Invalid parameters passed"); } - - LinearSolver::LinearSolver() = default; - } \ No newline at end of file diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 908726ca5d..d2d88914d3 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include namespace gtsam { @@ -45,13 +44,14 @@ namespace gtsam { * @param nonlinear optimizer parameters * @return pointer to a LinearSolver object */ - static std::shared_ptr fromNonlinearParams(const gtsam::NonlinearOptimizerParams &nlparams); virtual VectorValues solve(const GaussianFactorGraph &gfg) { throw std::runtime_error( "BUG_CHECK: Calling solve of the base class!"); }; + static std::shared_ptr fromLinearSolverParams(const LinearSolverParams ¶ms); + protected: LinearSolver(); diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 58f886204c..88715b851e 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -17,6 +17,9 @@ #pragma once +#include +#include + namespace gtsam { // TODO: Remove this enum @@ -31,9 +34,11 @@ namespace gtsam { EIGEN_QR, EIGEN_CHOLESKY, } LinearSolverType; -} - -class LinearSolverParams { + class LinearSolverParams { + public: + LinearSolverType solverType = MULTIFRONTAL_CHOLESKY; + boost::optional ordering; + }; -}; \ No newline at end of file +} \ No newline at end of file diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index ea88403823..a6113fceb8 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -33,7 +33,7 @@ namespace gtsam { /** * Eigen SparseSolver based Backend class */ - class SparseEigenSolver : public gtsam::LinearSolver { + class SparseEigenSolver : public LinearSolver { public: typedef enum { diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 18a8c37c8a..5ba33a0a3f 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -169,7 +169,10 @@ VectorValues NonlinearOptimizer::solve( "in LM solver ..."); } } else if (params.isEigenQR() || params.isEigenCholesky()) { - auto solver = LinearSolver::fromNonlinearParams(params); + LinearSolverParams lsparams; + lsparams.ordering = params.ordering; + lsparams.solverType = params.linearSolverType; + auto solver = LinearSolver::fromLinearSolverParams(lsparams); delta = solver->solve(gfg); } else { throw std::runtime_error( diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 91edd8f93a..aca4e1bb56 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -192,5 +192,4 @@ Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } - } // namespace diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index b34a6ef8c1..c1c54d14f1 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { @@ -75,7 +76,9 @@ class GTSAM_EXPORT NonlinearOptimizerParams { using LinearSolverType = gtsam::LinearSolverType; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) + std::shared_ptr linearSolverParams; + boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. inline bool isMultifrontal() const { From d9ee81490bbf3a957690b9084d6c1fb6120e9fb2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 31 Dec 2019 14:05:12 -0500 Subject: [PATCH 38/91] Revert "Removed redundant Scatter method" This reverts commit 8faaf32cc316cd2ff411cd75ec36ec96e9fb9567. --- gtsam/linear/Scatter.cpp | 11 ++++++++--- gtsam/linear/Scatter.h | 3 +++ gtsam/nonlinear/NonlinearFactorGraph.cpp | 7 +++---- 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 75385e3ac8..2ae86e5094 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -43,7 +43,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, // If we have an ordering, pre-fill the ordered variables first if (ordering) { for (Key key : *ordering) { - emplace_back(key, 0); + add(key, 0); } } @@ -56,7 +56,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, const JacobianFactor* asJacobian = dynamic_cast(factor.get()); if (asJacobian && asJacobian->cols() <= 1) continue; - // loop over variable keys + // loop over variables for (GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { const Key key = *variable; @@ -64,7 +64,7 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, if (it!=end()) it->dimension = factor->getDim(variable); else - emplace_back(key, factor->getDim(variable)); + add(key, factor->getDim(variable)); } } @@ -74,6 +74,11 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, if (first != end()) std::sort(first, end()); } +/* ************************************************************************* */ +void Scatter::add(Key key, size_t dim) { + emplace_back(SlotEntry(key, dim)); +} + /* ************************************************************************* */ FastVector::iterator Scatter::find(Key key) { iterator it = begin(); diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 9490df4c9a..68408e99be 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -57,6 +57,9 @@ class Scatter : public FastVector { /// Construct from gaussian factor graph, with (partial or complete) ordering GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); + /// Add a key/dim pair + void add(Key key, size_t dim); + private: /// Find the SlotEntry with the right key (linear time worst case) iterator find(Key key); diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 806a653b32..20a7c20134 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,8 +344,7 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values, - boost::optional ordering) { +static Scatter scatterFromValues(const Values& values, boost::optional ordering) { gttic(scatterFromValues); Scatter scatter; @@ -354,13 +353,13 @@ static Scatter scatterFromValues(const Values& values, if (!ordering) { // use "natural" ordering with keys taken from the initial values for (const auto& key_value : values) { - scatter.emplace_back(key_value.key, key_value.value.dim()); + scatter.add(key_value.key, key_value.value.dim()); } } else { // copy ordering into keys and lookup dimension in values, is O(n*log n) for (Key key : *ordering) { const Value& value = values.at(key); - scatter.emplace_back(key, value.dim()); + scatter.add(key, value.dim()); } } From 4551b656c3f94eb034385970738e0d521112e30c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 31 Dec 2019 14:42:31 -0500 Subject: [PATCH 39/91] Build fixes for rebasing to develop --- gtsam/linear/Scatter.cpp | 6 ++---- gtsam/linear/tests/testLinearSolver.cpp | 20 ++++++++------------ gtsam/nonlinear/NonlinearFactorGraph.cpp | 16 ++++------------ 3 files changed, 14 insertions(+), 28 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 2ae86e5094..07ecaf4838 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -41,10 +41,8 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, gttic(Scatter_Constructor); // If we have an ordering, pre-fill the ordered variables first - if (ordering) { - for (Key key : *ordering) { - add(key, 0); - } + for (Key key : ordering) { + add(key, 0); } // Now, find dimensions of variables and/or extend diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 81c682d424..de6530780d 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -49,13 +49,11 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::ERROR; - parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = EIGEN_QR; - parameters.ordering = Ordering::Colamd(A); + LinearSolverParams params; + params.solverType = EIGEN_QR; + params.ordering = Ordering::Colamd(A); - auto solver = LinearSolver::fromNonlinearParams(parameters); + auto solver = LinearSolver::fromLinearSolverParams(params); VectorValues actual = solver->solve(A); EXPECT(assert_equal(expected, actual)); } @@ -69,13 +67,11 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { expected.insert(0, Vector2(0.1, -0.2)); expected.insert(1, Vector2(-0.1, 0.1)); - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::ERROR; - parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = EIGEN_CHOLESKY; - parameters.ordering = Ordering::Colamd(A); + LinearSolverParams params; + params.solverType = EIGEN_CHOLESKY; + params.ordering = Ordering::Colamd(A); - auto solver = LinearSolver::fromNonlinearParams(parameters); + auto solver = LinearSolver::fromLinearSolverParams(params); VectorValues actual = solver->solve(A); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 20a7c20134..d4b9fbb68e 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -344,23 +344,15 @@ GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(const Values& li } /* ************************************************************************* */ -static Scatter scatterFromValues(const Values& values, boost::optional ordering) { +static Scatter scatterFromValues(const Values& values) { gttic(scatterFromValues); Scatter scatter; scatter.reserve(values.size()); - if (!ordering) { - // use "natural" ordering with keys taken from the initial values - for (const auto& key_value : values) { - scatter.add(key_value.key, key_value.value.dim()); - } - } else { - // copy ordering into keys and lookup dimension in values, is O(n*log n) - for (Key key : *ordering) { - const Value& value = values.at(key); - scatter.add(key, value.dim()); - } + // use "natural" ordering with keys taken from the initial values + for (const auto& key_value : values) { + scatter.add(key_value.key, key_value.value.dim()); } return scatter; From ede5feb4da4a1301dbf6dbf13e36117188219cfb Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 May 2020 15:11:16 -0400 Subject: [PATCH 40/91] Slight refactor --- gtsam/linear/LinearSolver.cpp | 8 ++------ gtsam/linear/LinearSolverParams.cpp | 19 ++++++++++++++++--- 2 files changed, 18 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 4f42fffbc8..10125efe4c 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -23,16 +23,12 @@ namespace gtsam { LinearSolver::LinearSolver() = default; std::shared_ptr LinearSolver::fromLinearSolverParams(const LinearSolverParams ¶ms) { - - boost::optional optionalOrdering; - if (params.ordering) optionalOrdering.reset(*params.ordering); - if (params.solverType == EIGEN_QR) { return std::shared_ptr( - new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *optionalOrdering)); + new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); } else if (params.solverType == EIGEN_CHOLESKY) { return std::shared_ptr( - new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *optionalOrdering)); + new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); } throw std::runtime_error( diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index faf370b304..506694ea5e 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -1,5 +1,18 @@ -// -// Created by fan on 9/8/19. -// +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LinearSolverParams.cpp + * @brief Linear Solver Parameters + * @author Fan Jiang + */ #include "LinearSolverParams.h" From 7df1be0552797b20d8471d1169d5d583998f9c6a Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 13 May 2020 15:57:59 -0400 Subject: [PATCH 41/91] Removed obsolete optionals --- gtsam/linear/GaussianFactorGraph.cpp | 34 +++++++++++++++++----------- gtsam/linear/GaussianFactorGraph.h | 18 +++++++++++++-- gtsam/linear/LinearSolver.h | 13 +++++++---- gtsam/linear/SparseEigenSolver.cpp | 2 +- 4 files changed, 46 insertions(+), 21 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index cd7ed7642c..b2d835bbeb 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -99,10 +99,18 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + vector > + GaussianFactorGraph::sparseJacobian() const { + Ordering ord(this->keys()); + + return sparseJacobian(ord); + } + /* ************************************************************************* */ vector > GaussianFactorGraph::sparseJacobian( - boost::optional ordering) const { + const Ordering& ordering) const { gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable std::map dims; @@ -118,16 +126,9 @@ namespace gtsam { // Compute first scalar column of each variable size_t currentColIndex = 0; std::map columnIndices; - if (ordering) { - for (const auto key : *ordering) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } - } else { - for (const auto& keyValue : dims) { - columnIndices[keyValue.first] = currentColIndex; - currentColIndex += keyValue.second; - } + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; } // Iterate over all factors, adding sparse scalar entries @@ -181,12 +182,19 @@ namespace gtsam { return entries; } + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_() const { + Ordering ord = Ordering::Natural(*this); + + return sparseJacobian_(ord); + } + /* ************************************************************************* */ Matrix GaussianFactorGraph::sparseJacobian_( - boost::optional optionalOrdering) const { + const Ordering& ordering) const { // call sparseJacobian typedef boost::tuple triplet; - vector result = sparseJacobian(optionalOrdering); + vector result = sparseJacobian(ordering); // translate to base 1 matrix size_t nzmax = result.size(); diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 0e20eb3096..5c48f4c58d 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -180,20 +180,34 @@ namespace gtsam { ///@name Linear Algebra ///@{ + /** + * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, + * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. + * The standard deviations are baked into A and b + */ + std::vector > sparseJacobian() const; + /** * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. * The standard deviations are baked into A and b */ std::vector > sparseJacobian( - boost::optional optionalOrdering = boost::none) const; + const Ordering& ordering) const; + + /** + * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries + * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. + * The standard deviations are baked into A and b + */ + Matrix sparseJacobian_() const; /** * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * The standard deviations are baked into A and b */ - Matrix sparseJacobian_(boost::optional optionalOrdering = boost::none) const; + Matrix sparseJacobian_(const Ordering& ordering) const; /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index d2d88914d3..1fd9ae067a 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -38,18 +38,21 @@ namespace gtsam { return false; }; - /// /** - * Factor method for generating a LinearSolver from legacy NonlinearOptimizerParams - * @param nonlinear optimizer parameters - * @return pointer to a LinearSolver object + * Solve a Gaussian Factor Graph with the solver + * @param gfg the GFG to be optimized + * @return the optimization result in VectorValues */ - virtual VectorValues solve(const GaussianFactorGraph &gfg) { throw std::runtime_error( "BUG_CHECK: Calling solve of the base class!"); }; + /** + * Factor method for generating a LinearSolver from legacy NonlinearOptimizerParams + * @param nonlinear optimizer parameters + * @return pointer to a LinearSolver object + */ static std::shared_ptr fromLinearSolverParams(const LinearSolverParams ¶ms); protected: diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index bb5e806166..55cc934eb5 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -34,7 +34,7 @@ namespace gtsam { /// obtain sparse matrix for eigen sparse solver std::pair obtainSparseMatrix( const GaussianFactorGraph &gfg, - boost::optional ordering = boost::none) { + const Ordering& ordering) { gttic_(EigenOptimizer_obtainSparseMatrix); From d9958f994665c6f37013822815de12242b1a8abd Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 17 May 2020 18:59:51 -0400 Subject: [PATCH 42/91] Add string option --- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index aca4e1bb56..f0e01deb9f 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -160,6 +160,10 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve return Iterative; if (linearSolverType == "CHOLMOD") return CHOLMOD; + if (linearSolverType == "EIGEN_CHOLESKY") + return EIGEN_CHOLESKY; + if (linearSolverType == "EIGEN_QR") + return EIGEN_QR; throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } From 9d8e8cc539c8d6e55e3f67eb5f34ae108c0f72d0 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Thu, 21 May 2020 14:43:20 -0400 Subject: [PATCH 43/91] Formatting and filename --- gtsam/linear/LinearSolverParams.h | 47 +++++++++++++++---------------- 1 file changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 88715b851e..a4a5191432 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -10,35 +10,34 @@ * -------------------------------------------------------------------------- */ /** - * @file LinearSolver.h - * @brief Common Interface for Linear Solvers + * @file LinearSolverParams.h + * @brief Parameters base class forLinear Solvers * @author Fan Jiang */ #pragma once -#include -#include +#include + +#include namespace gtsam { - // TODO: Remove this enum - /** See NonlinearOptimizerParams::linearSolverType */ - typedef enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - EIGEN_QR, - EIGEN_CHOLESKY, - } LinearSolverType; - - class LinearSolverParams { - public: - LinearSolverType solverType = MULTIFRONTAL_CHOLESKY; - boost::optional ordering; - }; - -} \ No newline at end of file +// Type of solver +typedef enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + Iterative, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + EIGEN_QR, + EIGEN_CHOLESKY, +} LinearSolverType; + +struct LinearSolverParams { + LinearSolverType solverType = MULTIFRONTAL_CHOLESKY; + boost::optional ordering; +}; + +} // namespace gtsam \ No newline at end of file From d2111664d812e848ab15b9191fb30003d8ec7da5 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Thu, 21 May 2020 15:16:21 -0400 Subject: [PATCH 44/91] Use boost::shared_ptr --- gtsam/linear/LinearSolver.cpp | 29 +++++++++++----------- gtsam/nonlinear/NonlinearOptimizerParams.h | 2 +- 2 files changed, 15 insertions(+), 16 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 10125efe4c..b0b9e3d93e 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -20,19 +20,18 @@ namespace gtsam { - LinearSolver::LinearSolver() = default; - - std::shared_ptr LinearSolver::fromLinearSolverParams(const LinearSolverParams ¶ms) { - if (params.solverType == EIGEN_QR) { - return std::shared_ptr( - new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); - } else if (params.solverType == EIGEN_CHOLESKY) { - return std::shared_ptr( - new SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); - } - - throw std::runtime_error( - "Invalid parameters passed"); - +LinearSolver::LinearSolver() = default; + +boost::shared_ptr LinearSolver::fromLinearSolverParams( + const LinearSolverParams ¶ms) { + if (params.solverType == EIGEN_QR) { + return boost::shared_ptr(new SparseEigenSolver( + SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); + } else if (params.solverType == EIGEN_CHOLESKY) { + return boost::shared_ptr(new SparseEigenSolver( + SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); } -} \ No newline at end of file + + throw std::runtime_error("Invalid parameters passed"); +} +} // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index c1c54d14f1..b663b5f4b6 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -76,7 +76,7 @@ class GTSAM_EXPORT NonlinearOptimizerParams { using LinearSolverType = gtsam::LinearSolverType; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - std::shared_ptr linearSolverParams; + boost::shared_ptr linearSolverParams; boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. From 59bf77a70da28690a2fb4e180ce679620797bbe6 Mon Sep 17 00:00:00 2001 From: Frank dellaert Date: Thu, 21 May 2020 15:26:42 -0400 Subject: [PATCH 45/91] Swiched to boost::shared_ptr --- gtsam/linear/LinearSolver.h | 74 ++++++++++++++++++------------------- 1 file changed, 36 insertions(+), 38 deletions(-) diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 1fd9ae067a..b8acc432c3 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -22,43 +22,41 @@ namespace gtsam { - /** Common Interface Class for all linear solvers */ - class LinearSolver { - public: - LinearSolver(LinearSolver &) = delete; - - // TODO: Remove this and use trait functions instead? - gtsam::LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of this instance - - virtual bool isIterative() { - return false; - }; - - virtual bool isSequential() { - return false; - }; - - /** - * Solve a Gaussian Factor Graph with the solver - * @param gfg the GFG to be optimized - * @return the optimization result in VectorValues - */ - virtual VectorValues solve(const GaussianFactorGraph &gfg) { - throw std::runtime_error( - "BUG_CHECK: Calling solve of the base class!"); - }; - - /** - * Factor method for generating a LinearSolver from legacy NonlinearOptimizerParams - * @param nonlinear optimizer parameters - * @return pointer to a LinearSolver object - */ - static std::shared_ptr fromLinearSolverParams(const LinearSolverParams ¶ms); - - protected: - LinearSolver(); - - virtual ~LinearSolver() = default; +/** Common Interface Class for all linear solvers */ +class LinearSolver { + public: + LinearSolver(LinearSolver &) = delete; + + // TODO: Remove this and use trait functions instead? + gtsam::LinearSolverType linearSolverType = + MULTIFRONTAL_CHOLESKY; ///< The type of this instance + + virtual bool isIterative() { return false; }; + + virtual bool isSequential() { return false; }; + + /** + * Solve a Gaussian Factor Graph with the solver + * @param gfg the GFG to be optimized + * @return the optimization result in VectorValues + */ + virtual VectorValues solve(const GaussianFactorGraph &gfg) { + throw std::runtime_error("BUG_CHECK: Calling solve of the base class!"); }; -} \ No newline at end of file + /** + * Factor method for generating a LinearSolver from legacy + * NonlinearOptimizerParams + * @param nonlinear optimizer parameters + * @return pointer to a LinearSolver object + */ + static boost::shared_ptr fromLinearSolverParams( + const LinearSolverParams ¶ms); + + protected: + LinearSolver(); + + virtual ~LinearSolver() = default; +}; + +} // namespace gtsam \ No newline at end of file From b7b527416e3a3f5fd38bc8da909e03f16b751007 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sat, 23 May 2020 15:06:43 -0400 Subject: [PATCH 46/91] Make Nonlinear Subclass of Linear --- gtsam/linear/LinearSolver.cpp | 4 +- gtsam/linear/LinearSolverParams.cpp | 84 +++++++++++++++++++ gtsam/linear/LinearSolverParams.h | 82 ++++++++++++++++++- gtsam/linear/tests/testLinearSolver.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.cpp | 79 ------------------ gtsam/nonlinear/NonlinearOptimizerParams.h | 86 +------------------- 7 files changed, 171 insertions(+), 170 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index b0b9e3d93e..4da7004d83 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -24,10 +24,10 @@ LinearSolver::LinearSolver() = default; boost::shared_ptr LinearSolver::fromLinearSolverParams( const LinearSolverParams ¶ms) { - if (params.solverType == EIGEN_QR) { + if (params.linearSolverType == EIGEN_QR) { return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); - } else if (params.solverType == EIGEN_CHOLESKY) { + } else if (params.linearSolverType == EIGEN_CHOLESKY) { return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); } diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index 506694ea5e..2d3b7def76 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -16,3 +16,87 @@ */ #include "LinearSolverParams.h" + +namespace gtsam { + +/* ************************************************************************* */ +void LinearSolverParams::setIterativeParams( + const boost::shared_ptr params) { + iterativeParams = params; +} + + +/* ************************************************************************* */ +std::string LinearSolverParams::linearSolverTranslator( + LinearSolverType linearSolverType) const { + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + return "MULTIFRONTAL_CHOLESKY"; + case MULTIFRONTAL_QR: + return "MULTIFRONTAL_QR"; + case SEQUENTIAL_CHOLESKY: + return "SEQUENTIAL_CHOLESKY"; + case SEQUENTIAL_QR: + return "SEQUENTIAL_QR"; + case Iterative: + return "ITERATIVE"; + case CHOLMOD: + return "CHOLMOD"; + default: + throw std::invalid_argument( + "Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } +} + +/* ************************************************************************* */ +LinearSolverType LinearSolverParams::linearSolverTranslator( + const std::string &linearSolverType) const { + if (linearSolverType == "MULTIFRONTAL_CHOLESKY") + return MULTIFRONTAL_CHOLESKY; + if (linearSolverType == "MULTIFRONTAL_QR") + return MULTIFRONTAL_QR; + if (linearSolverType == "SEQUENTIAL_CHOLESKY") + return SEQUENTIAL_CHOLESKY; + if (linearSolverType == "SEQUENTIAL_QR") + return SEQUENTIAL_QR; + if (linearSolverType == "ITERATIVE") + return Iterative; + if (linearSolverType == "CHOLMOD") + return CHOLMOD; + if (linearSolverType == "EIGEN_CHOLESKY") + return EIGEN_CHOLESKY; + if (linearSolverType == "EIGEN_QR") + return EIGEN_QR; + throw std::invalid_argument( + "Unknown linear solver type in SuccessiveLinearizationOptimizer"); +} + +/* ************************************************************************* */ +std::string LinearSolverParams::orderingTypeTranslator( + Ordering::OrderingType type) const { + switch (type) { + case Ordering::METIS: + return "METIS"; + case Ordering::COLAMD: + return "COLAMD"; + default: + if (ordering) + return "CUSTOM"; + else + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + } +} + +/* ************************************************************************* */ +Ordering::OrderingType LinearSolverParams::orderingTypeTranslator( + const std::string &type) const { + if (type == "METIS") + return Ordering::METIS; + if (type == "COLAMD") + return Ordering::COLAMD; + throw std::invalid_argument( + "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); +} + +} \ No newline at end of file diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index a4a5191432..548c6d031f 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -17,7 +17,9 @@ #pragma once +#include #include +#include #include @@ -36,8 +38,84 @@ typedef enum LinearSolverType { } LinearSolverType; struct LinearSolverParams { - LinearSolverType solverType = MULTIFRONTAL_CHOLESKY; - boost::optional ordering; +public: + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer + Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) + boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + + inline bool isMultifrontal() const { + return (linearSolverType == MULTIFRONTAL_CHOLESKY) + || (linearSolverType == MULTIFRONTAL_QR); + } + + inline bool isSequential() const { + return (linearSolverType == SEQUENTIAL_CHOLESKY) + || (linearSolverType == SEQUENTIAL_QR); + } + + inline bool isCholmod() const { + return (linearSolverType == CHOLMOD); + } + + inline bool isIterative() const { + return (linearSolverType == Iterative); + } + + inline bool isEigenQR() const { + return (linearSolverType == EIGEN_QR); + } + + inline bool isEigenCholesky() const { + return (linearSolverType == EIGEN_CHOLESKY); + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + case SEQUENTIAL_CHOLESKY: + return EliminatePreferCholesky; + + case MULTIFRONTAL_QR: + case SEQUENTIAL_QR: + return EliminateQR; + + default: + throw std::runtime_error( + "Nonlinear optimization parameter \"factorization\" is invalid"); + } + } + + std::string getLinearSolverType() const { + return linearSolverTranslator(linearSolverType); + } + + void setLinearSolverType(const std::string& solver) { + linearSolverType = linearSolverTranslator(solver); + } + + void setIterativeParams(const boost::shared_ptr params); + + void setOrdering(const Ordering& ordering) { + this->ordering = ordering; + this->orderingType = Ordering::CUSTOM; + } + + std::string getOrderingType() const { + return orderingTypeTranslator(orderingType); + } + + // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type + void setOrderingType(const std::string& ordering){ + orderingType = orderingTypeTranslator(ordering); + } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const; + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; + std::string orderingTypeTranslator(Ordering::OrderingType type) const; + Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index de6530780d..58b53c45de 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -50,7 +50,7 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.solverType = EIGEN_QR; + params.linearSolverType = EIGEN_QR; params.ordering = Ordering::Colamd(A); auto solver = LinearSolver::fromLinearSolverParams(params); @@ -68,7 +68,7 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.solverType = EIGEN_CHOLESKY; + params.linearSolverType = EIGEN_CHOLESKY; params.ordering = Ordering::Colamd(A); auto solver = LinearSolver::fromLinearSolverParams(params); diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 6426aea905..d2b1c82d34 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -170,7 +170,7 @@ VectorValues NonlinearOptimizer::solve( } else if (params.isEigenQR() || params.isEigenCholesky()) { LinearSolverParams lsparams; lsparams.ordering = params.ordering; - lsparams.solverType = params.linearSolverType; + lsparams.linearSolverType = params.linearSolverType; auto solver = LinearSolver::fromLinearSolverParams(lsparams); delta = solver->solve(gfg); } else { diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index f0e01deb9f..7712a6c324 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -65,12 +65,6 @@ std::string NonlinearOptimizerParams::verbosityTranslator( return s; } -/* ************************************************************************* */ -void NonlinearOptimizerParams::setIterativeParams( - const boost::shared_ptr params) { - iterativeParams = params; -} - /* ************************************************************************* */ void NonlinearOptimizerParams::print(const std::string& str) const { @@ -123,77 +117,4 @@ void NonlinearOptimizerParams::print(const std::string& str) const { std::cout.flush(); } -/* ************************************************************************* */ -std::string NonlinearOptimizerParams::linearSolverTranslator( - LinearSolverType linearSolverType) const { - switch (linearSolverType) { - case MULTIFRONTAL_CHOLESKY: - return "MULTIFRONTAL_CHOLESKY"; - case MULTIFRONTAL_QR: - return "MULTIFRONTAL_QR"; - case SEQUENTIAL_CHOLESKY: - return "SEQUENTIAL_CHOLESKY"; - case SEQUENTIAL_QR: - return "SEQUENTIAL_QR"; - case Iterative: - return "ITERATIVE"; - case CHOLMOD: - return "CHOLMOD"; - default: - throw std::invalid_argument( - "Unknown linear solver type in SuccessiveLinearizationOptimizer"); - } -} - -/* ************************************************************************* */ -NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolverTranslator( - const std::string& linearSolverType) const { - if (linearSolverType == "MULTIFRONTAL_CHOLESKY") - return MULTIFRONTAL_CHOLESKY; - if (linearSolverType == "MULTIFRONTAL_QR") - return MULTIFRONTAL_QR; - if (linearSolverType == "SEQUENTIAL_CHOLESKY") - return SEQUENTIAL_CHOLESKY; - if (linearSolverType == "SEQUENTIAL_QR") - return SEQUENTIAL_QR; - if (linearSolverType == "ITERATIVE") - return Iterative; - if (linearSolverType == "CHOLMOD") - return CHOLMOD; - if (linearSolverType == "EIGEN_CHOLESKY") - return EIGEN_CHOLESKY; - if (linearSolverType == "EIGEN_QR") - return EIGEN_QR; - throw std::invalid_argument( - "Unknown linear solver type in SuccessiveLinearizationOptimizer"); -} - -/* ************************************************************************* */ -std::string NonlinearOptimizerParams::orderingTypeTranslator( - Ordering::OrderingType type) const { - switch (type) { - case Ordering::METIS: - return "METIS"; - case Ordering::COLAMD: - return "COLAMD"; - default: - if (ordering) - return "CUSTOM"; - else - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); - } -} - -/* ************************************************************************* */ -Ordering::OrderingType NonlinearOptimizerParams::orderingTypeTranslator( - const std::string& type) const { - if (type == "METIS") - return Ordering::METIS; - if (type == "COLAMD") - return Ordering::COLAMD; - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); -} - } // namespace diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index b663b5f4b6..28759d9466 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -33,7 +33,7 @@ namespace gtsam { /** The common parameters for Nonlinear optimizers. Most optimizers * deriving from NonlinearOptimizer also subclass the parameters. */ -class GTSAM_EXPORT NonlinearOptimizerParams { +class GTSAM_EXPORT NonlinearOptimizerParams: public LinearSolverParams { public: /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { @@ -45,12 +45,10 @@ class GTSAM_EXPORT NonlinearOptimizerParams { double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - Ordering::OrderingType orderingType; ///< The method of ordering use during variable elimination (default COLAMD) NonlinearOptimizerParams() : maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT), orderingType(Ordering::COLAMD), - linearSolverType(MULTIFRONTAL_CHOLESKY) {} + 0.0), verbosity(SILENT) {} virtual ~NonlinearOptimizerParams() { } @@ -72,86 +70,6 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; - - using LinearSolverType = gtsam::LinearSolverType; - - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::shared_ptr linearSolverParams; - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - - IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. - - inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) - || (linearSolverType == MULTIFRONTAL_QR); - } - - inline bool isSequential() const { - return (linearSolverType == SEQUENTIAL_CHOLESKY) - || (linearSolverType == SEQUENTIAL_QR); - } - - inline bool isCholmod() const { - return (linearSolverType == CHOLMOD); - } - - inline bool isIterative() const { - return (linearSolverType == Iterative); - } - - inline bool isEigenQR() const { - return (linearSolverType == EIGEN_QR); - } - - inline bool isEigenCholesky() const { - return (linearSolverType == EIGEN_CHOLESKY); - } - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - switch (linearSolverType) { - case MULTIFRONTAL_CHOLESKY: - case SEQUENTIAL_CHOLESKY: - return EliminatePreferCholesky; - - case MULTIFRONTAL_QR: - case SEQUENTIAL_QR: - return EliminateQR; - - default: - throw std::runtime_error( - "Nonlinear optimization parameter \"factorization\" is invalid"); - } - } - - std::string getLinearSolverType() const { - return linearSolverTranslator(linearSolverType); - } - - void setLinearSolverType(const std::string& solver) { - linearSolverType = linearSolverTranslator(solver); - } - - void setIterativeParams(const boost::shared_ptr params); - - void setOrdering(const Ordering& ordering) { - this->ordering = ordering; - this->orderingType = Ordering::CUSTOM; - } - - std::string getOrderingType() const { - return orderingTypeTranslator(orderingType); - } - - // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type - void setOrderingType(const std::string& ordering){ - orderingType = orderingTypeTranslator(ordering); - } - -private: - std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; // For backward compatibility: From 5c445e1be5bc4be4825c7ab0dd80b1536ac14898 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 28 May 2020 09:53:08 -0400 Subject: [PATCH 47/91] Revert API change --- gtsam/geometry/triangulation.cpp | 2 +- gtsam/linear/LinearSolver.cpp | 4 ++-- gtsam/linear/LinearSolver.h | 6 +++--- gtsam/linear/LinearSolverParams.cpp | 18 ++++++++-------- gtsam/linear/LinearSolverParams.h | 28 ++++++++++++------------- gtsam/linear/SparseEigenSolver.h | 2 +- gtsam/linear/tests/testLinearSolver.cpp | 4 ++-- tests/testNonlinearOptimizer.cpp | 8 +++---- tests/testPCGSolver.cpp | 6 +++--- 9 files changed, 39 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index a3cf68208c..a5d2e04cd4 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -82,7 +82,7 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, params.absoluteErrorTol = 1.0; params.verbosityLM = LevenbergMarquardtParams::SILENT; params.verbosity = NonlinearOptimizerParams::SILENT; - params.linearSolverType = MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 4da7004d83..f77f804a12 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -24,10 +24,10 @@ LinearSolver::LinearSolver() = default; boost::shared_ptr LinearSolver::fromLinearSolverParams( const LinearSolverParams ¶ms) { - if (params.linearSolverType == EIGEN_QR) { + if (params.linearSolverType == LinearSolverParams::EIGEN_QR) { return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); - } else if (params.linearSolverType == EIGEN_CHOLESKY) { + } else if (params.linearSolverType == LinearSolverParams::EIGEN_CHOLESKY) { return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); } diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index b8acc432c3..415ad9a79e 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -23,13 +23,13 @@ namespace gtsam { /** Common Interface Class for all linear solvers */ -class LinearSolver { +class GTSAM_EXPORT LinearSolver { public: LinearSolver(LinearSolver &) = delete; // TODO: Remove this and use trait functions instead? - gtsam::LinearSolverType linearSolverType = - MULTIFRONTAL_CHOLESKY; ///< The type of this instance + LinearSolverParams::LinearSolverType linearSolverType = + LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of this instance virtual bool isIterative() { return false; }; diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index 2d3b7def76..4925b874e7 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -49,24 +49,24 @@ std::string LinearSolverParams::linearSolverTranslator( } /* ************************************************************************* */ -LinearSolverType LinearSolverParams::linearSolverTranslator( +LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( const std::string &linearSolverType) const { if (linearSolverType == "MULTIFRONTAL_CHOLESKY") - return MULTIFRONTAL_CHOLESKY; + return LinearSolverParams::MULTIFRONTAL_CHOLESKY; if (linearSolverType == "MULTIFRONTAL_QR") - return MULTIFRONTAL_QR; + return LinearSolverParams::MULTIFRONTAL_QR; if (linearSolverType == "SEQUENTIAL_CHOLESKY") - return SEQUENTIAL_CHOLESKY; + return LinearSolverParams::SEQUENTIAL_CHOLESKY; if (linearSolverType == "SEQUENTIAL_QR") - return SEQUENTIAL_QR; + return LinearSolverParams::SEQUENTIAL_QR; if (linearSolverType == "ITERATIVE") - return Iterative; + return LinearSolverParams::Iterative; if (linearSolverType == "CHOLMOD") - return CHOLMOD; + return LinearSolverParams::CHOLMOD; if (linearSolverType == "EIGEN_CHOLESKY") - return EIGEN_CHOLESKY; + return LinearSolverParams::EIGEN_CHOLESKY; if (linearSolverType == "EIGEN_QR") - return EIGEN_QR; + return LinearSolverParams::EIGEN_QR; throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 548c6d031f..c9b97d4a92 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -25,21 +25,21 @@ namespace gtsam { -// Type of solver -typedef enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - EIGEN_QR, - EIGEN_CHOLESKY, -} LinearSolverType; - -struct LinearSolverParams { +struct GTSAM_EXPORT LinearSolverParams { public: - LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer + // Type of solver + typedef enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + Iterative, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + EIGEN_QR, + EIGEN_CHOLESKY, + } LinearSolverType; + + LinearSolverType linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index a6113fceb8..9c27afcaa9 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -33,7 +33,7 @@ namespace gtsam { /** * Eigen SparseSolver based Backend class */ - class SparseEigenSolver : public LinearSolver { + class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { public: typedef enum { diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 58b53c45de..13ffc8b00b 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -50,7 +50,7 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.linearSolverType = EIGEN_QR; + params.linearSolverType = LinearSolverParams::EIGEN_QR; params.ordering = Ordering::Colamd(A); auto solver = LinearSolver::fromLinearSolverParams(params); @@ -68,7 +68,7 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.linearSolverType = EIGEN_CHOLESKY; + params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; params.ordering = Ordering::Colamd(A); auto solver = LinearSolver::fromLinearSolverParams(params); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index c3f638a3f5..bc6f61f333 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -165,22 +165,22 @@ TEST(NonlinearOptimizer, optimization_method) { LevenbergMarquardtParams params; // Multifrontal QR, will be parallel if TBB installed - params.linearSolverType = MULTIFRONTAL_QR; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR; Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); // Multifrontal Cholesky (more sensitive to conditioning, but faster) - params.linearSolverType = MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); // Test sparse Eigen QR solver - params.linearSolverType = EIGEN_QR; + params.linearSolverType = NonlinearOptimizerParams::EIGEN_QR; Values actualEigenQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenQR), tol); // Test sparse Eigen Cholesky solver - params.linearSolverType = EIGEN_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::EIGEN_CHOLESKY; Values actualEigenCholesky = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actualEigenCholesky), tol); } diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index b5c6e5b8bf..3271cdcd20 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -128,7 +128,7 @@ TEST( GaussianFactorGraphSystem, multiply_getb) // Test Dummy Preconditioner TEST(PCGSolver, dummy) { LevenbergMarquardtParams params; - params.linearSolverType = Iterative; + params.linearSolverType = NonlinearOptimizerParams::Iterative; auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); params.iterativeParams = pcg; @@ -148,7 +148,7 @@ TEST(PCGSolver, dummy) { // Test Block-Jacobi Precondioner TEST(PCGSolver, blockjacobi) { LevenbergMarquardtParams params; - params.linearSolverType = Iterative; + params.linearSolverType = NonlinearOptimizerParams::Iterative; auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); @@ -169,7 +169,7 @@ TEST(PCGSolver, blockjacobi) { // Test Incremental Subgraph PCG Solver TEST(PCGSolver, subgraph) { LevenbergMarquardtParams params; - params.linearSolverType = Iterative; + params.linearSolverType = NonlinearOptimizerParams::Iterative; auto pcg = boost::make_shared(); pcg->preconditioner_ = boost::make_shared(); params.iterativeParams = pcg; From c0dd826acfea748caf375fdc589af4dce8261cda Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 28 May 2020 10:25:06 -0400 Subject: [PATCH 48/91] Fix examples --- examples/Pose2SLAMwSPCG.cpp | 2 +- examples/SFMExample_SmartFactorPCG.cpp | 2 +- examples/SolverComparer.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index d20e5dcbc2..cd74b2c071 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -68,7 +68,7 @@ int main(int argc, char** argv) { // below We indicate that an iterative linear solver should be used. In // addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) - parameters.linearSolverType = Iterative; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; parameters.iterativeParams = boost::make_shared(); LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 066ae63881..3f553efc6f 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -88,7 +88,7 @@ int main(int argc, char* argv[]) { // used. In addition, the *type* of the iterativeParams decides on the type of // iterative solver, in this case the SPCG (subgraph PCG) LevenbergMarquardtParams parameters; - parameters.linearSolverType = Iterative; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; parameters.absoluteErrorTol = 1e-10; parameters.relativeErrorTol = 1e-10; parameters.maxIterations = 500; diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 3c44c1e18a..d1155fe4cf 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -475,7 +475,7 @@ void runBatch() gttic_(Create_optimizer); GaussNewtonParams params; - params.linearSolverType = MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; GaussNewtonOptimizer optimizer(measurements, initial, params); gttoc_(Create_optimizer); double lastError; From a44b07acb1b9f269b1286d660bb7087038d5e6c2 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 31 May 2020 17:39:51 -0400 Subject: [PATCH 49/91] Fixed the Sparse Eigen Solver --- gtsam/linear/SparseEigenSolver.cpp | 141 ++++++++++++++++++++++++++--- timing/timeSFMBAL.h | 4 +- 2 files changed, 131 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 55cc934eb5..02bc2b5d76 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -25,22 +25,105 @@ #include #include +#include + using namespace std; namespace gtsam { - using SpMat = Eigen::SparseMatrix; + using SpMat = Eigen::SparseMatrix; + + Eigen::SparseMatrix + sparseJacobianEigen( + const GaussianFactorGraph &gfg, + const Ordering &ordering) { + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); + } + } + + // Compute first scalar column of each variable + size_t currentColIndex = 0; + std::map columnIndices; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + + // Iterate over all factors, adding sparse scalar entries + vector> entries; + entries.reserve(60 * gfg.size()); + + size_t row = 0; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) continue; + + // Convert to JacobianFactor if necessary + JacobianFactor::shared_ptr jacobianFactor( + boost::dynamic_pointer_cast(factor)); + if (!jacobianFactor) { + HessianFactor::shared_ptr hessian( + boost::dynamic_pointer_cast(factor)); + if (hessian) + jacobianFactor.reset(new JacobianFactor(*hessian)); + else + throw invalid_argument( + "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + + // Whiten the factor and add entries for it + // iterate over all variables in the factor + const JacobianFactor whitened(jacobianFactor->whiten()); + for (JacobianFactor::const_iterator key = whitened.begin(); + key < whitened.end(); ++key) { + JacobianFactor::constABlock whitenedA = whitened.getA(key); + // find first column index for this key + size_t column_start = columnIndices[*key]; + for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) + for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { + double s = whitenedA(i, j); + if (std::abs(s) > 1e-12) + entries.emplace_back(row + i, column_start + j, s); + } + } + + JacobianFactor::constBVector whitenedb(whitened.getb()); + size_t bcolumn = currentColIndex; + for (size_t i = 0; i < (size_t) whitenedb.size(); i++) { + double s = whitenedb(i); + if (std::abs(s) > 1e-12) + entries.emplace_back(row + i, bcolumn, s); + } + + // Increment row index + row += jacobianFactor->rows(); + } + + // ...and make a sparse matrix with it. + Eigen::SparseMatrix Ab(row + 1, currentColIndex + 1); + Ab.setFromTriplets(entries.begin(), entries.end()); + Ab.makeCompressed(); + return Ab; + } + /// obtain sparse matrix for eigen sparse solver std::pair obtainSparseMatrix( const GaussianFactorGraph &gfg, - const Ordering& ordering) { + const Ordering &ordering) { gttic_(EigenOptimizer_obtainSparseMatrix); // Get sparse entries of Jacobian [A|b] augmented with RHS b. auto entries = gfg.sparseJacobian(ordering); + gttic_(EigenOptimizer_convertSparse); // Convert boost tuples to Eigen triplets vector> triplets; triplets.reserve(entries.size()); @@ -56,6 +139,7 @@ namespace gtsam { SpMat Ab(rows + 1, cols + 1); Ab.setFromTriplets(triplets.begin(), triplets.end()); Ab.makeCompressed(); + gttoc_(EigenOptimizer_convertSparse); gttoc_(EigenOptimizer_obtainSparseMatrix); @@ -87,28 +171,61 @@ namespace gtsam { return VectorValues(x, gfg.getKeyDimMap()); } else if (solverType == CHOLESKY) { - gttic_(EigenOptimizer_optimizeEigenCholesky); - auto Ab_pair = obtainSparseMatrix(gfg, ordering); - auto A = Ab_pair.first; - - gttic_(EigenOptimizer_optimizeEigenCholesky_Atranspose); +// gttic_(EigenOptimizer_optimizeEigenCholesky); +// SpMat Ab = sparseJacobianEigen(gfg, ordering); +// auto rows = Ab.rows(), cols = Ab.cols(); +// auto A = Ab.block(0, 0, rows, cols - 1); +// auto At = A.transpose(); + auto Ab = obtainSparseMatrix(gfg, ordering); + auto A = Ab.first; + auto b = Ab.second; auto At = A.transpose(); - gttoc_(EigenOptimizer_optimizeEigenCholesky_Atranspose); +// SpMat AtA(A.cols(), A.cols()); +// +// AtA.selfadjointView().rankUpdate(At); gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); - // Solve A*x = b using sparse QR from Eigen + // Solve A*x = b using sparse Cholesky from Eigen Eigen::SimplicialLDLT> solver(At * A); gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); gttic_(EigenOptimizer_optimizeEigenCholesky_solve); - Eigen::VectorXd x = solver.solve(At * Ab_pair.second); + Eigen::VectorXd x = solver.solve(At * b); gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); - return VectorValues(x, gfg.getKeyDimMap()); + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); + } + } + + VectorValues vv; + + std::map columnIndices; + + { + size_t currentColIndex = 0; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + } + + for (const pair keyDim : dims) { + vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + } + + return vv; } - return VectorValues(); // Should throw? + throw std::exception(); } SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) { diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 548c4de700..5ab86ede11 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -67,8 +67,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); -// params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); -// params.setVerbosityLM("SUMMARY"); + params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); + params.setVerbosityLM("SUMMARY"); if (gUseSchur) { // Create Schur-complement ordering From 5d4cc6d5cc4ca3428f7ab9cccba852bc19c4c7f8 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 31 May 2020 22:13:01 -0400 Subject: [PATCH 50/91] More optimization --- gtsam/linear/SparseEigenSolver.cpp | 22 +++++++++------------- timing/timeSFMBAL.h | 2 +- 2 files changed, 10 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 02bc2b5d76..9423e8b05b 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -108,7 +108,6 @@ namespace gtsam { // ...and make a sparse matrix with it. Eigen::SparseMatrix Ab(row + 1, currentColIndex + 1); Ab.setFromTriplets(entries.begin(), entries.end()); - Ab.makeCompressed(); return Ab; } @@ -171,23 +170,20 @@ namespace gtsam { return VectorValues(x, gfg.getKeyDimMap()); } else if (solverType == CHOLESKY) { -// gttic_(EigenOptimizer_optimizeEigenCholesky); -// SpMat Ab = sparseJacobianEigen(gfg, ordering); -// auto rows = Ab.rows(), cols = Ab.cols(); -// auto A = Ab.block(0, 0, rows, cols - 1); -// auto At = A.transpose(); - auto Ab = obtainSparseMatrix(gfg, ordering); - auto A = Ab.first; - auto b = Ab.second; + gttic_(EigenOptimizer_optimizeEigenCholesky); + SpMat Ab = sparseJacobianEigen(gfg, ordering); + auto rows = Ab.rows(), cols = Ab.cols(); + auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); -// SpMat AtA(A.cols(), A.cols()); -// -// AtA.selfadjointView().rankUpdate(At); + auto b = Ab.col(cols - 1); + + SpMat AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); // Solve A*x = b using sparse Cholesky from Eigen Eigen::SimplicialLDLT> - solver(At * A); + solver(AtA); gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); gttic_(EigenOptimizer_optimizeEigenCholesky_solve); diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 5ab86ede11..762b66c14f 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -67,7 +67,7 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); - params.setLinearSolverType("SEQUENTIAL_CHOLESKY"); + params.setLinearSolverType("EIGEN_CHOLESKY"); params.setVerbosityLM("SUMMARY"); if (gUseSchur) { From 9dc0029e162673042f4cdb404fb72e13148b8933 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 31 May 2020 23:21:48 -0400 Subject: [PATCH 51/91] Typos --- gtsam/linear/LinearSolverParams.h | 2 +- gtsam/linear/SparseEigenSolver.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index c9b97d4a92..c32376c0bd 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -11,7 +11,7 @@ /** * @file LinearSolverParams.h - * @brief Parameters base class forLinear Solvers + * @brief Parameters base class for Linear Solvers * @author Fan Jiang */ diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 9423e8b05b..6152a30b65 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -184,6 +184,7 @@ namespace gtsam { // Solve A*x = b using sparse Cholesky from Eigen Eigen::SimplicialLDLT> solver(AtA); + gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); gttic_(EigenOptimizer_optimizeEigenCholesky_solve); From dff2385c140664895cb60ecc1e90fad2deb5bd27 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 31 May 2020 23:57:49 -0400 Subject: [PATCH 52/91] Remove redundant check --- gtsam/linear/tests/testSparseEigenSolver.cpp | 77 -------------------- 1 file changed, 77 deletions(-) delete mode 100644 gtsam/linear/tests/testSparseEigenSolver.cpp diff --git a/gtsam/linear/tests/testSparseEigenSolver.cpp b/gtsam/linear/tests/testSparseEigenSolver.cpp deleted file mode 100644 index 69f9601c70..0000000000 --- a/gtsam/linear/tests/testSparseEigenSolver.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testEigenOptimizer.cpp - * @brief Unit tests for Eigen optimizer - * @author Mandy Xie - * @author Frank Dellaert - **/ - -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -/// Factor graph with 2 2D factors on 3 2D variables -static GaussianFactorGraph createSimpleGaussianFactorGraph() { - GaussianFactorGraph fg; - Key x1 = 2, x2 = 0, l1 = 1; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(x1, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(x2, 10 * I_2x2, x1, -10 * I_2x2, Vector2(2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(l1, 5 * I_2x2, x1, -5 * I_2x2, Vector2(0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(x2, -5 * I_2x2, l1, 5 * I_2x2, Vector2(-1.0, 1.5), unit2); - return fg; -} - -/* ************************************************************************* */ -TEST(EigenOptimizer, optimizeEigenQR) { - GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - - VectorValues expected; - expected.insert(2, Vector2(-0.1, -0.1)); - expected.insert(0, Vector2(0.1, -0.2)); - expected.insert(1, Vector2(-0.1, 0.1)); - - auto solver = new SparseEigenSolver(SparseEigenSolver::QR, Ordering::Colamd(A)); - VectorValues actual = solver->solve(A); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(EigenOptimizer, optimizeEigenCholesky) { - GaussianFactorGraph A = createSimpleGaussianFactorGraph(); - - VectorValues expected; - expected.insert(2, Vector2(-0.1, -0.1)); - expected.insert(0, Vector2(0.1, -0.2)); - expected.insert(1, Vector2(-0.1, 0.1)); - - auto solver = new SparseEigenSolver(SparseEigenSolver::CHOLESKY, Ordering::Colamd(A)); - VectorValues actual = solver->solve(A); - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ From ec7c91baf0b49204604ad64b3b9aff2d6ac6fe86 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 2 Jun 2020 12:29:53 -0400 Subject: [PATCH 53/91] Reduce overhead in SparseLDLT with ordering --- gtsam/linear/SparseEigenSolver.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 6152a30b65..d2b2db8954 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -22,7 +22,6 @@ #include #include -#include #include #include @@ -31,7 +30,7 @@ using namespace std; namespace gtsam { - using SpMat = Eigen::SparseMatrix; + using SpMat = Eigen::SparseMatrix; Eigen::SparseMatrix sparseJacobianEigen( @@ -161,7 +160,7 @@ namespace gtsam { // Solve A*x = b using sparse QR from Eigen gttic_(EigenOptimizer_optimizeEigenQR_create_solver); - Eigen::SparseQR> solver(Ab_pair.first); + Eigen::SparseQR> solver(Ab_pair.first); gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); gttic_(EigenOptimizer_optimizeEigenQR_solve); @@ -178,11 +177,11 @@ namespace gtsam { auto b = Ab.col(cols - 1); SpMat AtA(A.cols(), A.cols()); - AtA.selfadjointView().rankUpdate(At); + AtA.selfadjointView().rankUpdate(At); gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); // Solve A*x = b using sparse Cholesky from Eigen - Eigen::SimplicialLDLT> + Eigen::SimplicialLDLT> solver(AtA); gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); From b1912a5d754eccadd743b1e55f83916736694faa Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 7 Jun 2020 16:56:23 -0400 Subject: [PATCH 54/91] Add SuiteSparse (CHOLMOD) solver --- gtsam/CMakeLists.txt | 5 ++ gtsam/config.h.in | 3 + gtsam/linear/LinearSolver.cpp | 4 + gtsam/linear/LinearSolverParams.cpp | 8 ++ gtsam/linear/LinearSolverParams.h | 5 ++ gtsam/linear/SparseEigenSolver.cpp | 2 +- gtsam/linear/SparseEigenSolver.h | 3 + gtsam/linear/SuiteSparseSolver.cpp | 112 +++++++++++++++++++++++++ gtsam/linear/SuiteSparseSolver.h | 56 +++++++++++++ gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- 10 files changed, 198 insertions(+), 2 deletions(-) create mode 100644 gtsam/linear/SuiteSparseSolver.cpp create mode 100644 gtsam/linear/SuiteSparseSolver.h diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 3d1bbd2a79..f3c8202081 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -130,6 +130,11 @@ if(GTSAM_USE_TBB) target_include_directories(gtsam PUBLIC ${TBB_INCLUDE_DIRS}) endif() +# CHOLMOD include dir: +if (GTSAM_USE_SUITESPARSE) + target_include_directories(gtsam PUBLIC ${CHOLMOD_INCLUDE_DIR}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. diff --git a/gtsam/config.h.in b/gtsam/config.h.in index b480996ecc..f14431f08d 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -55,6 +55,9 @@ // Whether Eigen with MKL will use OpenMP (if OpenMP was found, Eigen uses MKL, and GTSAM_WITH_EIGEN_MKL_OPENMP is enabled in CMake) #cmakedefine GTSAM_USE_EIGEN_MKL_OPENMP +// Whether GTSAM will have the SuiteSparse solver +#cmakedefine GTSAM_USE_SUITESPARSE + // Eigen library version (needed to avoid mixing versions, which often leads // to segfaults) #cmakedefine GTSAM_EIGEN_VERSION_WORLD @GTSAM_EIGEN_VERSION_WORLD@ diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index f77f804a12..fe87915534 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -17,6 +17,7 @@ #include #include +#include namespace gtsam { @@ -30,6 +31,9 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( } else if (params.linearSolverType == LinearSolverParams::EIGEN_CHOLESKY) { return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); + } else if (params.linearSolverType == LinearSolverParams::SUITESPARSE_CHOLESKY) { + return boost::shared_ptr(new SuiteSparseSolver( + SuiteSparseSolver::SuiteSparseSolverType::CHOLESKY, *params.ordering)); } throw std::runtime_error("Invalid parameters passed"); diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index 4925b874e7..61fba7c611 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -42,6 +42,12 @@ std::string LinearSolverParams::linearSolverTranslator( return "ITERATIVE"; case CHOLMOD: return "CHOLMOD"; + case EIGEN_QR: + return "EIGEN_QR"; + case EIGEN_CHOLESKY: + return "EIGEN_CHOLESKY"; + case SUITESPARSE_CHOLESKY: + return "SUITESPARSE_CHOLESKY"; default: throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); @@ -67,6 +73,8 @@ LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( return LinearSolverParams::EIGEN_CHOLESKY; if (linearSolverType == "EIGEN_QR") return LinearSolverParams::EIGEN_QR; + if (linearSolverType == "SUITESPARSE_CHOLESKY") + return LinearSolverParams::SUITESPARSE_CHOLESKY; throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index c32376c0bd..a714e1490a 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -37,6 +37,7 @@ struct GTSAM_EXPORT LinearSolverParams { CHOLMOD, /* Experimental Flag */ EIGEN_QR, EIGEN_CHOLESKY, + SUITESPARSE_CHOLESKY, } LinearSolverType; LinearSolverType linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer @@ -71,6 +72,10 @@ struct GTSAM_EXPORT LinearSolverParams { return (linearSolverType == EIGEN_CHOLESKY); } + inline bool isSuiteSparseCholesky() const { + return (linearSolverType == SUITESPARSE_CHOLESKY); + } + GaussianFactorGraph::Eliminate getEliminationFunction() const { switch (linearSolverType) { case MULTIFRONTAL_CHOLESKY: diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index d2b2db8954..f0dfd83f31 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -33,7 +33,7 @@ namespace gtsam { using SpMat = Eigen::SparseMatrix; Eigen::SparseMatrix - sparseJacobianEigen( + SparseEigenSolver::sparseJacobianEigen( const GaussianFactorGraph &gfg, const Ordering &ordering) { // First find dimensions of each variable diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 9c27afcaa9..d71365864e 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -50,6 +50,9 @@ namespace gtsam { VectorValues solve(const GaussianFactorGraph &gfg) override; + static Eigen::SparseMatrix + sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering); + protected: SparseEigenSolverType solverType = QR; diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp new file mode 100644 index 0000000000..ff4880e74e --- /dev/null +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -0,0 +1,112 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SuiteSparseSolver.cpp + * + * @brief SuiteSparse based linear solver backend for GTSAM + * + * @date Jun 2020 + * @author Fan Jiang + */ + +#include "gtsam/linear/SuiteSparseSolver.h" + +#include "gtsam/linear/SparseEigenSolver.h" + +#ifdef GTSAM_USE_SUITESPARSE +#include +#endif + +namespace gtsam { + SuiteSparseSolver::SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, + const Ordering &ordering) { + solverType = type; + this->ordering = ordering; + } + + bool SuiteSparseSolver::isIterative() { + return false; + } + + bool SuiteSparseSolver::isSequential() { + return false; + } + +#ifdef GTSAM_USE_SUITESPARSE + VectorValues SuiteSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + if (solverType == QR) { + throw std::invalid_argument("This solver does not support QR."); + } else if (solverType == CHOLESKY) { + gttic_(SuiteSparseSolver_optimizeEigenCholesky); + Eigen::SparseMatrix + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); + auto rows = Ab.rows(), cols = Ab.cols(); + auto A = Ab.block(0, 0, rows, cols - 1); + auto At = A.transpose(); + auto b = Ab.col(cols - 1); + + Eigen::SparseMatrix + AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); + + gttic_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); + // Solve A*x = b using sparse Cholesky from Eigen + Eigen::CholmodSimplicialLDLT + , Eigen::Upper> + solver; + solver.compute(AtA); + gttoc_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); + + gttic_(SuiteSparseSolver_optimizeEigenCholesky_solve); + Matrix Atb = (At * b).eval(); + Eigen::VectorXd x = solver.solve(Atb); + gttoc_(SuiteSparseSolver_optimizeEigenCholesky_solve); + + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); + } + } + + VectorValues vv; + + std::map columnIndices; + + { + size_t currentColIndex = 0; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + } + + for (const std::pair keyDim : dims) { + vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + } + + return vv; + } + + throw std::exception(); + } +#else + VectorValues SuiteSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + throw std::invalid_argument("This GTSAM is compiled without Cholmod support"); + } +#endif +} diff --git a/gtsam/linear/SuiteSparseSolver.h b/gtsam/linear/SuiteSparseSolver.h new file mode 100644 index 0000000000..67310f390f --- /dev/null +++ b/gtsam/linear/SuiteSparseSolver.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SuiteSparseSolver.h + * + * @brief SuiteSparse based linear solver backend for GTSAM + * + * @date Jun 2020 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * Eigen SparseSolver based Backend class + */ + class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { + public: + + typedef enum { + QR, + CHOLESKY + } SuiteSparseSolverType; + + + explicit SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering); + + bool isIterative() override; + + bool isSequential() override; + + VectorValues solve(const GaussianFactorGraph &gfg) override; + + protected: + + SuiteSparseSolverType solverType = QR; + + Ordering ordering; + }; +} // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index d2b1c82d34..0b2ea06a2a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -167,7 +167,7 @@ VectorValues NonlinearOptimizer::solve( "NonlinearOptimizer::solve: special cg parameter type is not handled " "in LM solver ..."); } - } else if (params.isEigenQR() || params.isEigenCholesky()) { + } else if (params.isEigenQR() || params.isEigenCholesky() || params.isSuiteSparseCholesky()) { LinearSolverParams lsparams; lsparams.ordering = params.ordering; lsparams.linearSolverType = params.linearSolverType; From 30ac4a1d5ae0460874895a767f6990eb06ea01ee Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 7 Jun 2020 17:04:24 -0400 Subject: [PATCH 55/91] Working SuiteSparse solver --- CMakeLists.txt | 19 ++ cmake/FindSuiteSparse.cmake | 530 +++++++++++++++++++++++++++++ gtsam/linear/SuiteSparseSolver.cpp | 4 + timing/timeSFMBAL.h | 2 +- 4 files changed, 554 insertions(+), 1 deletion(-) create mode 100644 cmake/FindSuiteSparse.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index a810ac9df0..b6784bc08a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,6 +74,7 @@ option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency c option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) +option(GTSAM_WITH_SUITESPARSE "Build with the SuiteSparse linear solver (CHOLMOD)" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF) @@ -268,6 +269,17 @@ if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) endif() endif() +############################################################################### +# Find SuiteSparse +find_package(SuiteSparse COMPONENTS CHOLMOD) + +if(CHOLMOD_FOUND AND GTSAM_WITH_SUITESPARSE) + set(GTSAM_USE_SUITESPARSE 1) # This will go into config.h + message(STATUS "Found CHOLMOD at ${CHOLMOD_LIBRARY}") + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${CHOLMOD_LIBRARY}) +else() + set(GTSAM_USE_SUITESPARSE 0) +endif() ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen @@ -564,6 +576,13 @@ elseif(OPENMP_FOUND) else() message(STATUS " Eigen will use MKL and OpenMP : OpenMP not found") endif() +if(GTSAM_WITH_SUITESPARSE) + message(STATUS " GTSAM will use SuiteSparse : Yes") +elseif(SUITESPARSE_FOUND) + message(STATUS " GTSAM will use SuiteSparse : SuiteSparse found but GTSAM_WITH_SUITESPARSE is disabled") +else() + message(STATUS " GTSAM will use SuiteSparse : SuiteSparse not found") +endif() message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") if(GTSAM_THROW_CHEIRALITY_EXCEPTION) diff --git a/cmake/FindSuiteSparse.cmake b/cmake/FindSuiteSparse.cmake new file mode 100644 index 0000000000..e38333700c --- /dev/null +++ b/cmake/FindSuiteSparse.cmake @@ -0,0 +1,530 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindSuiteSparse.cmake - Find SuiteSparse libraries & dependencies. +# +# This module defines the following variables: +# +# SUITESPARSE_FOUND: TRUE iff SuiteSparse and all dependencies have been found. +# SUITESPARSE_INCLUDE_DIRS: Include directories for all SuiteSparse components. +# SUITESPARSE_LIBRARIES: Libraries for all SuiteSparse component libraries and +# dependencies. +# SUITESPARSE_VERSION: Extracted from UFconfig.h (<= v3) or +# SuiteSparse_config.h (>= v4). +# SUITESPARSE_MAIN_VERSION: Equal to 4 if SUITESPARSE_VERSION = 4.2.1 +# SUITESPARSE_SUB_VERSION: Equal to 2 if SUITESPARSE_VERSION = 4.2.1 +# SUITESPARSE_SUBSUB_VERSION: Equal to 1 if SUITESPARSE_VERSION = 4.2.1 +# +# SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION: TRUE iff running +# on Ubuntu, SUITESPARSE_VERSION is 3.4.0 and found SuiteSparse is a system +# install, in which case found version of SuiteSparse cannot be used to link +# a shared library due to a bug (static linking is unaffected). +# +# The following variables control the behaviour of this module: +# +# SUITESPARSE_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for SuiteSparse includes, +# e.g: /timbuktu/include. +# SUITESPARSE_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for SuiteSparse libraries, +# e.g: /timbuktu/lib. +# +# The following variables define the presence / includes & libraries for the +# SuiteSparse components searched for, the SUITESPARSE_XX variables are the +# union of the variables for all components. +# +# == Symmetric Approximate Minimum Degree (AMD) +# AMD_FOUND +# AMD_INCLUDE_DIR +# AMD_LIBRARY +# +# == Constrained Approximate Minimum Degree (CAMD) +# CAMD_FOUND +# CAMD_INCLUDE_DIR +# CAMD_LIBRARY +# +# == Column Approximate Minimum Degree (COLAMD) +# COLAMD_FOUND +# COLAMD_INCLUDE_DIR +# COLAMD_LIBRARY +# +# Constrained Column Approximate Minimum Degree (CCOLAMD) +# CCOLAMD_FOUND +# CCOLAMD_INCLUDE_DIR +# CCOLAMD_LIBRARY +# +# == Sparse Supernodal Cholesky Factorization and Update/Downdate (CHOLMOD) +# CHOLMOD_FOUND +# CHOLMOD_INCLUDE_DIR +# CHOLMOD_LIBRARY +# +# == Multifrontal Sparse QR (SuiteSparseQR) +# SUITESPARSEQR_FOUND +# SUITESPARSEQR_INCLUDE_DIR +# SUITESPARSEQR_LIBRARY +# +# == Common configuration for all but CSparse (SuiteSparse version >= 4). +# SUITESPARSE_CONFIG_FOUND +# SUITESPARSE_CONFIG_INCLUDE_DIR +# SUITESPARSE_CONFIG_LIBRARY +# +# == Common configuration for all but CSparse (SuiteSparse version < 4). +# UFCONFIG_FOUND +# UFCONFIG_INCLUDE_DIR +# +# Optional SuiteSparse Dependencies: +# +# == Serial Graph Partitioning and Fill-reducing Matrix Ordering (METIS) +# METIS_FOUND +# METIS_LIBRARY + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindSuiteSparse was invoked. +macro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) + if (MSVC) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) +endmacro(SUITESPARSE_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find SuiteSparse or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(SUITESPARSE_REPORT_NOT_FOUND REASON_MSG) + unset(SUITESPARSE_FOUND) + unset(SUITESPARSE_INCLUDE_DIRS) + unset(SUITESPARSE_LIBRARIES) + unset(SUITESPARSE_VERSION) + unset(SUITESPARSE_MAIN_VERSION) + unset(SUITESPARSE_SUB_VERSION) + unset(SUITESPARSE_SUBSUB_VERSION) + # Do NOT unset SUITESPARSE_FOUND_REQUIRED_VARS here, as it is used by + # FindPackageHandleStandardArgs() to generate the automatic error message on + # failure which highlights which components are missing. + + suitesparse_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (SuiteSparse_FIND_QUIETLY) + message(STATUS "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + elseif (SuiteSparse_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find SuiteSparse - " ${REASON_MSG} ${ARGN}) + endif (SuiteSparse_FIND_QUIETLY) + + # Do not call return(), s/t we keep processing if not called with REQUIRED + # and report all missing components, rather than bailing after failing to find + # the first. +endmacro(SUITESPARSE_REPORT_NOT_FOUND) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set SUITESPARSE_FOUND, but +# not the other variables we require / set here which could cause the search +# logic here to fail. +unset(SUITESPARSE_FOUND) + +# Handle possible presence of lib prefix for libraries on MSVC, see +# also SUITESPARSE_RESET_FIND_LIBRARY_PREFIX(). +if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") +endif (MSVC) + +# On macOS, add the Homebrew prefix (with appropriate suffixes) to the +# respective HINTS directories (after any user-specified locations). This +# handles Homebrew installations into non-standard locations (not /usr/local). +# We do not use CMAKE_PREFIX_PATH for this as given the search ordering of +# find_xxx(), doing so would override any user-specified HINTS locations with +# the Homebrew version if it exists. +if (CMAKE_SYSTEM_NAME MATCHES "Darwin") + find_program(HOMEBREW_EXECUTABLE brew) + mark_as_advanced(FORCE HOMEBREW_EXECUTABLE) + if (HOMEBREW_EXECUTABLE) + # Detected a Homebrew install, query for its install prefix. + execute_process(COMMAND ${HOMEBREW_EXECUTABLE} --prefix + OUTPUT_VARIABLE HOMEBREW_INSTALL_PREFIX + OUTPUT_STRIP_TRAILING_WHITESPACE) + message(STATUS "Detected Homebrew with install prefix: " + "${HOMEBREW_INSTALL_PREFIX}, adding to CMake search paths.") + list(APPEND SUITESPARSE_INCLUDE_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/include") + list(APPEND SUITESPARSE_LIBRARY_DIR_HINTS "${HOMEBREW_INSTALL_PREFIX}/lib") + endif() +endif() + +# Specify search directories for include files and libraries (this is the union +# of the search directories for all OSs). Search user-specified hint +# directories first if supplied, and search user-installed locations first +# so that we prefer user installs to system installs where both exist. +list(APPEND SUITESPARSE_CHECK_INCLUDE_DIRS + /opt/local/include + /opt/local/include/ufsparse # Mac OS X + /usr/local/homebrew/include # Mac OS X + /usr/local/include + /usr/include) +list(APPEND SUITESPARSE_CHECK_LIBRARY_DIRS + /opt/local/lib + /opt/local/lib/ufsparse # Mac OS X + /usr/local/homebrew/lib # Mac OS X + /usr/local/lib + /usr/lib) +# Additional suffixes to try appending to each search path. +list(APPEND SUITESPARSE_CHECK_PATH_SUFFIXES + suitesparse) # Windows/Ubuntu + +# Wrappers to find_path/library that pass the SuiteSparse search hints/paths. +# +# suitesparse_find_component( [FILES name1 [name2 ...]] +# [LIBRARIES name1 [name2 ...]] +# [REQUIRED]) +macro(suitesparse_find_component COMPONENT) + include(CMakeParseArguments) + set(OPTIONS REQUIRED) + set(MULTI_VALUE_ARGS FILES LIBRARIES) + cmake_parse_arguments(SUITESPARSE_FIND_${COMPONENT} + "${OPTIONS}" "" "${MULTI_VALUE_ARGS}" ${ARGN}) + + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS ${COMPONENT}_FOUND) + endif() + + set(${COMPONENT}_FOUND TRUE) + if (SUITESPARSE_FIND_${COMPONENT}_FILES) + find_path(${COMPONENT}_INCLUDE_DIR + NAMES ${SUITESPARSE_FIND_${COMPONENT}_FILES} + HINTS ${SUITESPARSE_INCLUDE_DIR_HINTS} + PATHS ${SUITESPARSE_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) + if (${COMPONENT}_INCLUDE_DIR) + message(STATUS "Found ${COMPONENT} headers in: " + "${${COMPONENT}_INCLUDE_DIR}") + mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + else() + # Specified headers not found. + set(${COMPONENT}_FOUND FALSE) + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + suitesparse_report_not_found( + "Did not find ${COMPONENT} header (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} header (optional " + "SuiteSparse component).") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(${COMPONENT}_INCLUDE_DIR) + endif() + endif() + endif() + + if (SUITESPARSE_FIND_${COMPONENT}_LIBRARIES) + find_library(${COMPONENT}_LIBRARY + NAMES ${SUITESPARSE_FIND_${COMPONENT}_LIBRARIES} + HINTS ${SUITESPARSE_LIBRARY_DIR_HINTS} + PATHS ${SUITESPARSE_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${SUITESPARSE_CHECK_PATH_SUFFIXES}) + if (${COMPONENT}_LIBRARY) + message(STATUS "Found ${COMPONENT} library: ${${COMPONENT}_LIBRARY}") + mark_as_advanced(${COMPONENT}_LIBRARY) + else () + # Specified libraries not found. + set(${COMPONENT}_FOUND FALSE) + if (SUITESPARSE_FIND_${COMPONENT}_REQUIRED) + suitesparse_report_not_found( + "Did not find ${COMPONENT} library (required SuiteSparse component).") + else() + message(STATUS "Did not find ${COMPONENT} library (optional SuiteSparse " + "dependency)") + # Hide optional vars from CMake GUI even if not found. + mark_as_advanced(${COMPONENT}_LIBRARY) + endif() + endif() + endif() +endmacro() + +# Given the number of components of SuiteSparse, and to ensure that the +# automatic failure message generated by FindPackageHandleStandardArgs() +# when not all required components are found is helpful, we maintain a list +# of all variables that must be defined for SuiteSparse to be considered found. +unset(SUITESPARSE_FOUND_REQUIRED_VARS) + +# BLAS. +find_package(BLAS QUIET) +if (NOT BLAS_FOUND) + suitesparse_report_not_found( + "Did not find BLAS library (required for SuiteSparse).") +endif (NOT BLAS_FOUND) +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS BLAS_FOUND) + +# LAPACK. +find_package(LAPACK QUIET) +if (NOT LAPACK_FOUND) + suitesparse_report_not_found( + "Did not find LAPACK library (required for SuiteSparse).") +endif (NOT LAPACK_FOUND) +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS LAPACK_FOUND) + +suitesparse_find_component(AMD REQUIRED FILES amd.h LIBRARIES amd) +suitesparse_find_component(CAMD REQUIRED FILES camd.h LIBRARIES camd) +suitesparse_find_component(COLAMD REQUIRED FILES colamd.h LIBRARIES colamd) +suitesparse_find_component(CCOLAMD REQUIRED FILES ccolamd.h LIBRARIES ccolamd) +suitesparse_find_component(CHOLMOD REQUIRED FILES cholmod.h LIBRARIES cholmod) +suitesparse_find_component( + SUITESPARSEQR REQUIRED FILES SuiteSparseQR.hpp LIBRARIES spqr) +if (SUITESPARSEQR_FOUND) + # SuiteSparseQR may be compiled with Intel Threading Building Blocks, + # we assume that if TBB is installed, SuiteSparseQR was compiled with + # support for it, this will do no harm if it wasn't. + find_package(TBB QUIET) + if (TBB_FOUND) + message(STATUS "Found Intel Thread Building Blocks (TBB) library " + "(${TBB_VERSION}) assuming SuiteSparseQR was compiled " + "with TBB.") + # Add the TBB libraries to the SuiteSparseQR libraries (the only + # libraries to optionally depend on TBB). + list(APPEND SUITESPARSEQR_LIBRARY ${TBB_LIBRARIES}) + else() + message(STATUS "Did not find Intel TBB library, assuming SuiteSparseQR was " + "not compiled with TBB.") + endif() +endif(SUITESPARSEQR_FOUND) + +# UFconfig / SuiteSparse_config. +# +# If SuiteSparse version is >= 4 then SuiteSparse_config is required. +# For SuiteSparse 3, UFconfig.h is required. +suitesparse_find_component( + SUITESPARSE_CONFIG FILES SuiteSparse_config.h LIBRARIES suitesparseconfig) + +if (SUITESPARSE_CONFIG_FOUND) + # SuiteSparse_config (SuiteSparse version >= 4) requires librt library for + # timing by default when compiled on Linux or Unix, but not on OSX (which + # does not have librt). + if (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) + suitesparse_find_component(LIBRT LIBRARIES rt) + if (LIBRT_FOUND) + message(STATUS "Adding librt: ${LIBRT_LIBRARY} to " + "SuiteSparse_config libraries (required on Linux & Unix [not OSX] if " + "SuiteSparse is compiled with timing).") + list(APPEND SUITESPARSE_CONFIG_LIBRARY ${LIBRT_LIBRARY}) + else() + message(STATUS "Could not find librt, but found SuiteSparse_config, " + "assuming that SuiteSparse was compiled without timing.") + endif () + endif (CMAKE_SYSTEM_NAME MATCHES "Linux" OR UNIX AND NOT APPLE) +else() + # Failed to find SuiteSparse_config (>= v4 installs), instead look for + # UFconfig header which should be present in < v4 installs. + suitesparse_find_component(UFCONFIG FILES UFconfig.h) +endif () + +if (NOT SUITESPARSE_CONFIG_FOUND AND + NOT UFCONFIG_FOUND) + suitesparse_report_not_found( + "Failed to find either: SuiteSparse_config header & library (should be " + "present in all SuiteSparse >= v4 installs), or UFconfig header (should " + "be present in all SuiteSparse < v4 installs).") +endif() + +# Extract the SuiteSparse version from the appropriate header (UFconfig.h for +# <= v3, SuiteSparse_config.h for >= v4). +list(APPEND SUITESPARSE_FOUND_REQUIRED_VARS SUITESPARSE_VERSION) + +if (UFCONFIG_FOUND) + # SuiteSparse version <= 3. + set(SUITESPARSE_VERSION_FILE ${UFCONFIG_INCLUDE_DIR}/UFconfig.h) + if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " + "information for <= v3 SuiteSparse installs, but UFconfig was found " + "(only present in <= v3 installs).") + else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + file(READ ${SUITESPARSE_VERSION_FILE} UFCONFIG_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" + SUITESPARSE_MAIN_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" + SUITESPARSE_SUB_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" + SUITESPARSE_SUBSUB_VERSION "${UFCONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SUITESPARSE_VERSION + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) +endif (UFCONFIG_FOUND) + +if (SUITESPARSE_CONFIG_FOUND) + # SuiteSparse version >= 4. + set(SUITESPARSE_VERSION_FILE + ${SUITESPARSE_CONFIG_INCLUDE_DIR}/SuiteSparse_config.h) + if (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + suitesparse_report_not_found( + "Could not find file: ${SUITESPARSE_VERSION_FILE} containing version " + "information for >= v4 SuiteSparse installs, but SuiteSparse_config was " + "found (only present in >= v4 installs).") + else (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) + file(READ ${SUITESPARSE_VERSION_FILE} SUITESPARSE_CONFIG_CONTENTS) + + string(REGEX MATCH "#define SUITESPARSE_MAIN_VERSION [0-9]+" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_MAIN_VERSION ([0-9]+)" "\\1" + SUITESPARSE_MAIN_VERSION "${SUITESPARSE_MAIN_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUB_VERSION [0-9]+" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUB_VERSION "${SUITESPARSE_SUB_VERSION}") + + string(REGEX MATCH "#define SUITESPARSE_SUBSUB_VERSION [0-9]+" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_CONFIG_CONTENTS}") + string(REGEX REPLACE "#define SUITESPARSE_SUBSUB_VERSION ([0-9]+)" "\\1" + SUITESPARSE_SUBSUB_VERSION "${SUITESPARSE_SUBSUB_VERSION}") + + # This is on a single line s/t CMake does not interpret it as a list of + # elements and insert ';' separators which would result in 4.;2.;1 nonsense. + set(SUITESPARSE_VERSION + "${SUITESPARSE_MAIN_VERSION}.${SUITESPARSE_SUB_VERSION}.${SUITESPARSE_SUBSUB_VERSION}") + endif (NOT EXISTS ${SUITESPARSE_VERSION_FILE}) +endif (SUITESPARSE_CONFIG_FOUND) + +# METIS (Optional dependency). +suitesparse_find_component(METIS LIBRARIES metis) + +# Only mark SuiteSparse as found if all required components and dependencies +# have been found. +set(SUITESPARSE_FOUND TRUE) +foreach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) + if (NOT ${REQUIRED_VAR}) + set(SUITESPARSE_FOUND FALSE) + endif (NOT ${REQUIRED_VAR}) +endforeach(REQUIRED_VAR ${SUITESPARSE_FOUND_REQUIRED_VARS}) + +if (SUITESPARSE_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${AMD_INCLUDE_DIR} + ${CAMD_INCLUDE_DIR} + ${COLAMD_INCLUDE_DIR} + ${CCOLAMD_INCLUDE_DIR} + ${CHOLMOD_INCLUDE_DIR} + ${SUITESPARSEQR_INCLUDE_DIR}) + # Handle config separately, as otherwise at least one of them will be set + # to NOTFOUND which would cause any check on SUITESPARSE_INCLUDE_DIRS to fail. + if (SUITESPARSE_CONFIG_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${SUITESPARSE_CONFIG_INCLUDE_DIR}) + endif (SUITESPARSE_CONFIG_FOUND) + if (UFCONFIG_FOUND) + list(APPEND SUITESPARSE_INCLUDE_DIRS + ${UFCONFIG_INCLUDE_DIR}) + endif (UFCONFIG_FOUND) + # As SuiteSparse includes are often all in the same directory, remove any + # repetitions. + list(REMOVE_DUPLICATES SUITESPARSE_INCLUDE_DIRS) + + # Important: The ordering of these libraries is *NOT* arbitrary, as these + # could potentially be static libraries their link ordering is important. + list(APPEND SUITESPARSE_LIBRARIES + ${SUITESPARSEQR_LIBRARY} + ${CHOLMOD_LIBRARY} + ${CCOLAMD_LIBRARY} + ${CAMD_LIBRARY} + ${COLAMD_LIBRARY} + ${AMD_LIBRARY} + ${LAPACK_LIBRARIES} + ${BLAS_LIBRARIES}) + if (SUITESPARSE_CONFIG_FOUND) + list(APPEND SUITESPARSE_LIBRARIES + ${SUITESPARSE_CONFIG_LIBRARY}) + endif (SUITESPARSE_CONFIG_FOUND) + if (METIS_FOUND) + list(APPEND SUITESPARSE_LIBRARIES + ${METIS_LIBRARY}) + endif (METIS_FOUND) +endif() + +# Determine if we are running on Ubuntu with the package install of SuiteSparse +# which is broken and does not support linking a shared library. +set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION FALSE) +if (CMAKE_SYSTEM_NAME MATCHES "Linux" AND + SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) + find_program(LSB_RELEASE_EXECUTABLE lsb_release) + if (LSB_RELEASE_EXECUTABLE) + # Any even moderately recent Ubuntu release (likely to be affected by + # this bug) should have lsb_release, if it isn't present we are likely + # on a different Linux distribution (should be fine). + execute_process(COMMAND ${LSB_RELEASE_EXECUTABLE} -si + OUTPUT_VARIABLE LSB_DISTRIBUTOR_ID + OUTPUT_STRIP_TRAILING_WHITESPACE) + + if (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND + SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") + # We are on Ubuntu, and the SuiteSparse version matches the broken + # system install version and is a system install. + set(SUITESPARSE_IS_BROKEN_SHARED_LINKING_UBUNTU_SYSTEM_VERSION TRUE) + message(STATUS "Found system install of SuiteSparse " + "${SUITESPARSE_VERSION} running on Ubuntu, which has a known bug " + "preventing linking of shared libraries (static linking unaffected).") + endif (LSB_DISTRIBUTOR_ID MATCHES "Ubuntu" AND + SUITESPARSE_LIBRARIES MATCHES "/usr/lib/libamd") + endif (LSB_RELEASE_EXECUTABLE) +endif (CMAKE_SYSTEM_NAME MATCHES "Linux" AND + SUITESPARSE_VERSION VERSION_EQUAL 3.4.0) + +suitesparse_reset_find_library_prefix() + +# Handle REQUIRED and QUIET arguments to FIND_PACKAGE +include(FindPackageHandleStandardArgs) +if (SUITESPARSE_FOUND) + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} + VERSION_VAR SUITESPARSE_VERSION + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") +else (SUITESPARSE_FOUND) + # Do not pass VERSION_VAR to FindPackageHandleStandardArgs() if we failed to + # find SuiteSparse to avoid a confusing autogenerated failure message + # that states 'not found (missing: FOO) (found version: x.y.z)'. + find_package_handle_standard_args(SuiteSparse + REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} + FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") +endif (SUITESPARSE_FOUND) \ No newline at end of file diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index ff4880e74e..c9d8842839 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -63,6 +63,10 @@ namespace gtsam { Eigen::CholmodSimplicialLDLT , Eigen::Upper> solver; + solver.cholmod().nmethods = 1; + solver.cholmod().method[0].ordering = CHOLMOD_NATURAL; + solver.cholmod().postorder = false; + solver.compute(AtA); gttoc_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); diff --git a/timing/timeSFMBAL.h b/timing/timeSFMBAL.h index 762b66c14f..bbeec14958 100644 --- a/timing/timeSFMBAL.h +++ b/timing/timeSFMBAL.h @@ -67,7 +67,7 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph, // Set parameters to be similar to ceres LevenbergMarquardtParams params; LevenbergMarquardtParams::SetCeresDefaults(¶ms); - params.setLinearSolverType("EIGEN_CHOLESKY"); + params.setLinearSolverType("SUITESPARSE_CHOLESKY"); params.setVerbosityLM("SUMMARY"); if (gUseSchur) { From 6f57e0e049142a756a59129ac40708cfc0921661 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Sun, 7 Jun 2020 19:59:39 -0400 Subject: [PATCH 56/91] Fix linking on Linux with SuiteSparse --- gtsam/3rdparty/CMakeLists.txt | 2 +- gtsam/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 89149d964e..065f0f7bf2 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -49,7 +49,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_SUPPORT_NESTED_DISSECTION) +if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) add_subdirectory(metis) endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index f3c8202081..5fd8a6f55b 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -88,7 +88,7 @@ configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) -if(GTSAM_SUPPORT_NESTED_DISSECTION) +if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) endif() From 988556b47317f0afb9b1e6b82a4d94d8ee98d19c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 8 Jun 2020 10:40:54 -0400 Subject: [PATCH 57/91] Add CUDA cuSparse solver --- CMakeLists.txt | 7 + cmake/FindCUDAToolkit.cmake | 733 +++++++++++++++++++++++++ cmake/FindTBB.cmake | 2 +- gtsam/CMakeLists.txt | 4 + gtsam/config.h.in | 3 + gtsam/linear/CuSparseSolver.cpp | 216 ++++++++ gtsam/linear/CuSparseSolver.h | 56 ++ gtsam/linear/LinearSolver.cpp | 4 + gtsam/linear/LinearSolverParams.cpp | 4 + gtsam/linear/LinearSolverParams.h | 5 + gtsam/linear/SuiteSparseSolver.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- 12 files changed, 1035 insertions(+), 3 deletions(-) create mode 100644 cmake/FindCUDAToolkit.cmake create mode 100644 gtsam/linear/CuSparseSolver.cpp create mode 100644 gtsam/linear/CuSparseSolver.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b6784bc08a..31765d2b4b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,7 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_WITH_SUITESPARSE "Build with the SuiteSparse linear solver (CHOLMOD)" ON) +option(GTSAM_WITH_CUSPARSE "Enable CUDA support" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON) option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typedef Point2 and Point3 to Eigen::Vector equivalents" OFF) @@ -281,6 +282,12 @@ else() set(GTSAM_USE_SUITESPARSE 0) endif() +if(GTSAM_WITH_CUSPARSE) + find_package(CUDAToolkit REQUIRED) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES CUDA::cusparse CUDA::cusolver) + set(GTSAM_USE_CUSPARSE 1) +endif() + ############################################################################### # Option for using system Eigen or GTSAM-bundled Eigen ### These patches only affect usage of MKL. If you want to enable MKL, you *must* diff --git a/cmake/FindCUDAToolkit.cmake b/cmake/FindCUDAToolkit.cmake new file mode 100644 index 0000000000..db302c8e56 --- /dev/null +++ b/cmake/FindCUDAToolkit.cmake @@ -0,0 +1,733 @@ +# Distributed under the OSI-approved BSD 3-Clause License. See accompanying +# file Copyright.txt or https://cmake.org/licensing for details. + +#[=======================================================================[.rst: +FindCUDAToolkit +--------------- + +This script locates the NVIDIA CUDA toolkit and the associated libraries, but +does not require the ``CUDA`` language be enabled for a given project. This +module does not search for the NVIDIA CUDA Samples. + +Search Behavior +^^^^^^^^^^^^^^^ + +Finding the CUDA Toolkit requires finding the ``nvcc`` executable, which is +searched for in the following order: + +1. If the ``CUDA`` language has been enabled we will use the directory + containing the compiler as the first search location for ``nvcc``. + +2. If the ``CUDAToolkit_ROOT`` cmake configuration variable (e.g., + ``-DCUDAToolkit_ROOT=/some/path``) *or* environment variable is defined, it + will be searched. If both an environment variable **and** a + configuration variable are specified, the *configuration* variable takes + precedence. + + The directory specified here must be such that the executable ``nvcc`` can be + found underneath the directory specified by ``CUDAToolkit_ROOT``. If + ``CUDAToolkit_ROOT`` is specified, but no ``nvcc`` is found underneath, this + package is marked as **not** found. No subsequent search attempts are + performed. + +3. If the CUDA_PATH environment variable is defined, it will be searched. + +4. The user's path is searched for ``nvcc`` using :command:`find_program`. If + this is found, no subsequent search attempts are performed. Users are + responsible for ensuring that the first ``nvcc`` to show up in the path is + the desired path in the event that multiple CUDA Toolkits are installed. + +5. On Unix systems, if the symbolic link ``/usr/local/cuda`` exists, this is + used. No subsequent search attempts are performed. No default symbolic link + location exists for the Windows platform. + +6. The platform specific default install locations are searched. If exactly one + candidate is found, this is used. The default CUDA Toolkit install locations + searched are: + + +-------------+-------------------------------------------------------------+ + | Platform | Search Pattern | + +=============+=============================================================+ + | macOS | ``/Developer/NVIDIA/CUDA-X.Y`` | + +-------------+-------------------------------------------------------------+ + | Other Unix | ``/usr/local/cuda-X.Y`` | + +-------------+-------------------------------------------------------------+ + | Windows | ``C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\vX.Y`` | + +-------------+-------------------------------------------------------------+ + + Where ``X.Y`` would be a specific version of the CUDA Toolkit, such as + ``/usr/local/cuda-9.0`` or + ``C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\v9.0`` + + .. note:: + + When multiple CUDA Toolkits are installed in the default location of a + system (e.g., both ``/usr/local/cuda-9.0`` and ``/usr/local/cuda-10.0`` + exist but the ``/usr/local/cuda`` symbolic link does **not** exist), this + package is marked as **not** found. + + There are too many factors involved in making an automatic decision in + the presence of multiple CUDA Toolkits being installed. In this + situation, users are encouraged to either (1) set ``CUDAToolkit_ROOT`` or + (2) ensure that the correct ``nvcc`` executable shows up in ``$PATH`` for + :command:`find_program` to find. + +Options +^^^^^^^ + +``VERSION`` + If specified, describes the version of the CUDA Toolkit to search for. + +``REQUIRED`` + If specified, configuration will error if a suitable CUDA Toolkit is not + found. + +``QUIET`` + If specified, the search for a suitable CUDA Toolkit will not produce any + messages. + +``EXACT`` + If specified, the CUDA Toolkit is considered found only if the exact + ``VERSION`` specified is recovered. + +Imported targets +^^^^^^^^^^^^^^^^ + +An :ref:`imported target ` named ``CUDA::toolkit`` is provided. + +This module defines :prop_tgt:`IMPORTED` targets for each +of the following libraries that are part of the CUDAToolkit: + +- :ref:`CUDA Runtime Library` +- :ref:`CUDA Driver Library` +- :ref:`cuBLAS` +- :ref:`cuFFT` +- :ref:`cuRAND` +- :ref:`cuSOLVER` +- :ref:`cuSPARSE` +- :ref:`cuPTI` +- :ref:`NPP` +- :ref:`nvBLAS` +- :ref:`nvGRAPH` +- :ref:`nvJPEG` +- :ref:`nvidia-ML` +- :ref:`nvRTC` +- :ref:`nvToolsExt` +- :ref:`OpenCL` +- :ref:`cuLIBOS` + +.. _`cuda_toolkit_rt_lib`: + +CUDA Runtime Library +"""""""""""""""""""" + +The CUDA Runtime library (cudart) are what most applications will typically +need to link against to make any calls such as `cudaMalloc`, and `cudaFree`. + +Targets Created: + +- ``CUDA::cudart`` +- ``CUDA::cudart_static`` + +.. _`cuda_toolkit_driver_lib`: + +CUDA Driver Library +"""""""""""""""""""" + +The CUDA Driver library (cuda) are used by applications that use calls +such as `cuMemAlloc`, and `cuMemFree`. This is generally used by advanced + + +Targets Created: + +- ``CUDA::cuda_driver`` +- ``CUDA::cuda_driver`` + +.. _`cuda_toolkit_cuBLAS`: + +cuBLAS +"""""" + +The `cuBLAS `_ library. + +Targets Created: + +- ``CUDA::cublas`` +- ``CUDA::cublas_static`` + +.. _`cuda_toolkit_cuFFT`: + +cuFFT +""""" +The `cuFFT `_ library. +Targets Created: +- ``CUDA::cufft`` +- ``CUDA::cufftw`` +- ``CUDA::cufft_static`` +- ``CUDA::cufftw_static`` +cuRAND +"""""" +The `cuRAND `_ library. +Targets Created: +- ``CUDA::curand`` +- ``CUDA::curand_static`` +.. _`cuda_toolkit_cuSOLVER`: +cuSOLVER +"""""""" +The `cuSOLVER `_ library. +Targets Created: +- ``CUDA::cusolver`` +- ``CUDA::cusolver_static`` +.. _`cuda_toolkit_cuSPARSE`: +cuSPARSE +"""""""" +The `cuSPARSE `_ library. +Targets Created: +- ``CUDA::cusparse`` +- ``CUDA::cusparse_static`` +.. _`cuda_toolkit_cupti`: +cupti +""""" + +The `NVIDIA CUDA Profiling Tools Interface `_. + +Targets Created: + +- ``CUDA::cupti`` +- ``CUDA::cupti_static`` + +.. _`cuda_toolkit_NPP`: + +NPP +""" +The `NPP `_ libraries. +Targets Created: +- `nppc`: + - ``CUDA::nppc`` + - ``CUDA::nppc_static`` +- `nppial`: Arithmetic and logical operation functions in `nppi_arithmetic_and_logical_operations.h` + - ``CUDA::nppial`` + - ``CUDA::nppial_static`` +- `nppicc`: Color conversion and sampling functions in `nppi_color_conversion.h` + - ``CUDA::nppicc`` + - ``CUDA::nppicc_static`` +- `nppicom`: JPEG compression and decompression functions in `nppi_compression_functions.h` + - ``CUDA::nppicom`` + - ``CUDA::nppicom_static`` +- `nppidei`: Data exchange and initialization functions in `nppi_data_exchange_and_initialization.h` + - ``CUDA::nppidei`` + - ``CUDA::nppidei_static`` +- `nppif`: Filtering and computer vision functions in `nppi_filter_functions.h` + - ``CUDA::nppif`` + - ``CUDA::nppif_static`` +- `nppig`: Geometry transformation functions found in `nppi_geometry_transforms.h` + - ``CUDA::nppig`` + - ``CUDA::nppig_static`` +- `nppim`: Morphological operation functions found in `nppi_morphological_operations.h` + - ``CUDA::nppim`` + - ``CUDA::nppim_static`` +- `nppist`: Statistics and linear transform in `nppi_statistics_functions.h` and `nppi_linear_transforms.h` + - ``CUDA::nppist`` + - ``CUDA::nppist_static`` +- `nppisu`: Memory support functions in `nppi_support_functions.h` + - ``CUDA::nppisu`` + - ``CUDA::nppisu_static`` +- `nppitc`: Threshold and compare operation functions in `nppi_threshold_and_compare_operations.h` + - ``CUDA::nppitc`` + - ``CUDA::nppitc_static`` +- `npps`: + - ``CUDA::npps`` + - ``CUDA::npps_static`` +.. _`cuda_toolkit_nvBLAS`: +nvBLAS +"""""" +The `nvBLAS `_ libraries. +This is a shared library only. +Targets Created: +- ``CUDA::nvblas`` +.. _`cuda_toolkit_nvGRAPH`: +nvGRAPH +""""""" + +The `nvGRAPH `_ library. + +Targets Created: + +- ``CUDA::nvgraph`` +- ``CUDA::nvgraph_static`` + + +.. _`cuda_toolkit_nvJPEG`: + +nvJPEG +"""""" + +The `nvJPEG `_ library. +Introduced in CUDA 10. + +Targets Created: + +- ``CUDA::nvjpeg`` +- ``CUDA::nvjpeg_static`` + +.. _`cuda_toolkit_nvRTC`: + +nvRTC +""""" +The `nvRTC `_ (Runtime Compilation) library. +This is a shared library only. +Targets Created: +- ``CUDA::nvrtc`` +.. _`cuda_toolkit_nvml`: +nvidia-ML +""""""""" + +The `NVIDIA Management Library `_. +This is a shared library only. + +Targets Created: + +- ``CUDA::nvml`` + +.. _`cuda_toolkit_nvToolsExt`: + +nvToolsExt +"""""""""" + +The `NVIDIA Tools Extension `_. +This is a shared library only. + +Targets Created: + +- ``CUDA::nvToolsExt`` + +.. _`cuda_toolkit_opencl`: + +OpenCL +"""""" + +The `NVIDIA OpenCL Library `_. +This is a shared library only. + +Targets Created: + +- ``CUDA::OpenCL`` + +.. _`cuda_toolkit_cuLIBOS`: + +cuLIBOS +""""""" +The cuLIBOS library is a backend thread abstraction layer library which is +static only. The ``CUDA::cublas_static``, ``CUDA::cusparse_static``, +``CUDA::cufft_static``, ``CUDA::curand_static``, and (when implemented) NPP +libraries all automatically have this dependency linked. +Target Created: +- ``CUDA::culibos`` +**Note**: direct usage of this target by consumers should not be necessary. +.. _`cuda_toolkit_cuRAND`: +Result variables +^^^^^^^^^^^^^^^^ +``CUDAToolkit_FOUND`` + A boolean specifying whether or not the CUDA Toolkit was found. +``CUDAToolkit_VERSION`` + The exact version of the CUDA Toolkit found (as reported by + ``nvcc --version``). +``CUDAToolkit_VERSION_MAJOR`` + The major version of the CUDA Toolkit. +``CUDAToolkit_VERSION_MAJOR`` + The minor version of the CUDA Toolkit. +``CUDAToolkit_VERSION_PATCH`` + The patch version of the CUDA Toolkit. +``CUDAToolkit_BIN_DIR`` + The path to the CUDA Toolkit library directory that contains the CUDA + executable ``nvcc``. +``CUDAToolkit_INCLUDE_DIRS`` + The path to the CUDA Toolkit ``include`` folder containing the header files + required to compile a project linking against CUDA. +``CUDAToolkit_LIBRARY_DIR`` + The path to the CUDA Toolkit library directory that contains the CUDA + Runtime library ``cudart``. +``CUDAToolkit_TARGET_DIR`` + The path to the CUDA Toolkit directory including the target architecture + when cross-compiling. When not cross-compiling this will be equivalant to + ``CUDAToolkit_ROOT_DIR``. +``CUDAToolkit_NVCC_EXECUTABLE`` + The path to the NVIDIA CUDA compiler ``nvcc``. Note that this path may + **not** be the same as + :variable:`CMAKE_CUDA_COMPILER _COMPILER>`. ``nvcc`` must be + found to determine the CUDA Toolkit version as well as determining other + features of the Toolkit. This variable is set for the convenience of + modules that depend on this one. +#]=======================================================================] +# NOTE: much of this was simply extracted from FindCUDA.cmake. +# James Bigler, NVIDIA Corp (nvidia.com - jbigler) +# Abe Stephens, SCI Institute -- http://www.sci.utah.edu/~abe/FindCuda.html +# +# Copyright (c) 2008 - 2009 NVIDIA Corporation. All rights reserved. +# +# Copyright (c) 2007-2009 +# Scientific Computing and Imaging Institute, University of Utah +# +# This code is licensed under the MIT License. See the FindCUDA.cmake script +# for the text of the license. +# The MIT License +# +# License for the specific language governing rights and limitations under +# Permission is hereby granted, free of charge, to any person obtaining a +# copy of this software and associated documentation files (the "Software"), +# to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, +# and/or sell copies of the Software, and to permit persons to whom the +# Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included +# in all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS +# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +# FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER +# DEALINGS IN THE SOFTWARE. +# +############################################################################### +if(CMAKE_CUDA_COMPILER_LOADED AND NOT CUDAToolkit_BIN_DIR) + get_filename_component(cuda_dir "${CMAKE_CUDA_COMPILER}" DIRECTORY) + # use the already detected cuda compiler + set(CUDAToolkit_BIN_DIR "${cuda_dir}" CACHE PATH "") + unset(cuda_dir) +endif() +# Try language- or user-provided path first. +if(CUDAToolkit_BIN_DIR) + find_program(CUDAToolkit_NVCC_EXECUTABLE + NAMES nvcc nvcc.exe + PATHS ${CUDAToolkit_BIN_DIR} + NO_DEFAULT_PATH + ) +endif() +# Search using CUDAToolkit_ROOT +find_program(CUDAToolkit_NVCC_EXECUTABLE + NAMES nvcc nvcc.exe + PATHS ENV CUDA_PATH + PATH_SUFFIXES bin + ) +# If the user specified CUDAToolkit_ROOT but nvcc could not be found, this is an error. +if (NOT CUDAToolkit_NVCC_EXECUTABLE AND (DEFINED CUDAToolkit_ROOT OR DEFINED ENV{CUDAToolkit_ROOT})) + # Declare error messages now, print later depending on find_package args. + set(fail_base "Could not find nvcc executable in path specified by") + set(cuda_root_fail "${fail_base} CUDAToolkit_ROOT=${CUDAToolkit_ROOT}") + set(env_cuda_root_fail "${fail_base} environment variable CUDAToolkit_ROOT=$ENV{CUDAToolkit_ROOT}") + if (CUDAToolkit_FIND_REQUIRED) + if (DEFINED CUDAToolkit_ROOT) + message(FATAL_ERROR ${cuda_root_fail}) + elseif (DEFINED ENV{CUDAToolkit_ROOT}) + message(FATAL_ERROR ${env_cuda_root_fail}) + endif() + else() + if (NOT CUDAToolkit_FIND_QUIETLY) + if (DEFINED CUDAToolkit_ROOT) + message(STATUS ${cuda_root_fail}) + elseif (DEFINED ENV{CUDAToolkit_ROOT}) + message(STATUS ${env_cuda_root_fail}) + endif() + endif() + set(CUDAToolkit_FOUND FALSE) + unset(fail_base) + unset(cuda_root_fail) + unset(env_cuda_root_fail) + return() + endif() +endif() +# CUDAToolkit_ROOT cmake / env variable not specified, try platform defaults. +# +# - Linux: /usr/local/cuda-X.Y +# - macOS: /Developer/NVIDIA/CUDA-X.Y +# - Windows: C:\Program Files\NVIDIA GPU Computing Toolkit\CUDA\vX.Y +# +# We will also search the default symlink location /usr/local/cuda first since +# if CUDAToolkit_ROOT is not specified, it is assumed that the symlinked +# directory is the desired location. +if (NOT CUDAToolkit_NVCC_EXECUTABLE) + if (UNIX) + if (NOT APPLE) + set(platform_base "/usr/local/cuda-") + else() + set(platform_base "/Developer/NVIDIA/CUDA-") + endif() + else() + set(platform_base "C:\\Program Files\\NVIDIA GPU Computing Toolkit\\CUDA\\v") + endif() + # Build out a descending list of possible cuda installations, e.g. + file(GLOB possible_paths "${platform_base}*") + # Iterate the glob results and create a descending list. + set(possible_versions) + foreach (p ${possible_paths}) + # Extract version number from end of string + string(REGEX MATCH "[0-9][0-9]?\\.[0-9]$" p_version ${p}) + if (IS_DIRECTORY ${p} AND p_version) + list(APPEND possible_versions ${p_version}) + endif() + endforeach() + # Cannot use list(SORT) because that is alphabetical, we need numerical. + # NOTE: this is not an efficient sorting strategy. But even if a user had + # every possible version of CUDA installed, this wouldn't create any + # significant overhead. + set(versions) + foreach (v ${possible_versions}) + list(LENGTH versions num_versions) + # First version, nothing to compare with so just append. + if (num_versions EQUAL 0) + list(APPEND versions ${v}) + else() + # Loop through list. Insert at an index when comparison is + # VERSION_GREATER since we want a descending list. Duplicates will not + # happen since this came from a glob list of directories. + set(i 0) + set(early_terminate FALSE) + while (i LESS num_versions) + list(GET versions ${i} curr) + if (v VERSION_GREATER curr) + list(INSERT versions ${i} ${v}) + set(early_terminate TRUE) + break() + endif() + math(EXPR i "${i} + 1") + endwhile() + # If it did not get inserted, place it at the end. + if (NOT early_terminate) + list(APPEND versions ${v}) + endif() + endif() + endforeach() + # With a descending list of versions, populate possible paths to search. + set(search_paths) + foreach (v ${versions}) + list(APPEND search_paths "${platform_base}${v}") + endforeach() + # Force the global default /usr/local/cuda to the front on Unix. + if (UNIX) + list(INSERT search_paths 0 "/usr/local/cuda") + endif() + # Now search for nvcc again using the platform default search paths. + find_program(CUDAToolkit_NVCC_EXECUTABLE + NAMES nvcc nvcc.exe + PATHS ${search_paths} + PATH_SUFFIXES bin + ) + # We are done with these variables now, cleanup for caller. + unset(platform_base) + unset(possible_paths) + unset(possible_versions) + unset(versions) + unset(i) + unset(early_terminate) + unset(search_paths) + if (NOT CUDAToolkit_NVCC_EXECUTABLE) + if (CUDAToolkit_FIND_REQUIRED) + message(FATAL_ERROR "Could not find nvcc, please set CUDAToolkit_ROOT.") + elseif(NOT CUDAToolkit_FIND_QUIETLY) + message(STATUS "Could not find nvcc, please set CUDAToolkit_ROOT.") + endif() + set(CUDAToolkit_FOUND FALSE) + return() + endif() +endif() +if(NOT CUDAToolkit_BIN_DIR AND CUDAToolkit_NVCC_EXECUTABLE) + get_filename_component(cuda_dir "${CUDAToolkit_NVCC_EXECUTABLE}" DIRECTORY) + set(CUDAToolkit_BIN_DIR "${cuda_dir}" CACHE PATH "" FORCE) + unset(cuda_dir) +endif() +if(CUDAToolkit_NVCC_EXECUTABLE AND + CUDAToolkit_NVCC_EXECUTABLE STREQUAL CMAKE_CUDA_COMPILER) + # Need to set these based off the already computed CMAKE_CUDA_COMPILER_VERSION value + # This if statement will always match, but is used to provide variables for MATCH 1,2,3... + if(CMAKE_CUDA_COMPILER_VERSION MATCHES [=[([0-9]+)\.([0-9]+)\.([0-9]+)]=]) + set(CUDAToolkit_VERSION_MAJOR "${CMAKE_MATCH_1}") + set(CUDAToolkit_VERSION_MINOR "${CMAKE_MATCH_2}") + set(CUDAToolkit_VERSION_PATCH "${CMAKE_MATCH_3}") + set(CUDAToolkit_VERSION "${CMAKE_CUDA_COMPILER_VERSION}") + endif() +else() + # Compute the version by invoking nvcc + execute_process (COMMAND ${CUDAToolkit_NVCC_EXECUTABLE} "--version" OUTPUT_VARIABLE NVCC_OUT) + if(NVCC_OUT MATCHES [=[ V([0-9]+)\.([0-9]+)\.([0-9]+)]=]) + set(CUDAToolkit_VERSION_MAJOR "${CMAKE_MATCH_1}") + set(CUDAToolkit_VERSION_MINOR "${CMAKE_MATCH_2}") + set(CUDAToolkit_VERSION_PATCH "${CMAKE_MATCH_3}") + set(CUDAToolkit_VERSION "${CMAKE_MATCH_1}.${CMAKE_MATCH_2}.${CMAKE_MATCH_3}") + endif() + unset(NVCC_OUT) +endif() +get_filename_component(CUDAToolkit_ROOT_DIR ${CUDAToolkit_BIN_DIR} DIRECTORY ABSOLUTE) +# Handle cross compilation +if(CMAKE_CROSSCOMPILING) + if(CMAKE_SYSTEM_PROCESSOR STREQUAL "armv7-a") + # Support for NVPACK + set (CUDAToolkit_TARGET_NAME "armv7-linux-androideabi") + elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "arm") + # Support for arm cross compilation + set(CUDAToolkit_TARGET_NAME "armv7-linux-gnueabihf") + elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64") + # Support for aarch64 cross compilation + if (ANDROID_ARCH_NAME STREQUAL "arm64") + set(CUDAToolkit_TARGET_NAME "aarch64-linux-androideabi") + else() + set(CUDAToolkit_TARGET_NAME "aarch64-linux") + endif (ANDROID_ARCH_NAME STREQUAL "arm64") + elseif(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set(CUDAToolkit_TARGET_NAME "x86_64-linux") + endif() + if (EXISTS "${CUDAToolkit_ROOT_DIR}/targets/${CUDAToolkit_TARGET_NAME}") + set(CUDAToolkit_TARGET_DIR "${CUDAToolkit_ROOT_DIR}/targets/${CUDAToolkit_TARGET_NAME}") + # add known CUDA target root path to the set of directories we search for programs, libraries and headers + list(PREPEND CMAKE_FIND_ROOT_PATH "${CUDAToolkit_TARGET_DIR}") + # Mark that we need to pop the root search path changes after we have + # found all cuda libraries so that searches for our cross-compilation + # libraries work when another cuda sdk is in CMAKE_PREFIX_PATH or + # PATh + set(_CUDAToolkit_Pop_ROOT_PATH True) + endif() +else() + # Not cross compiling + set(CUDAToolkit_TARGET_DIR "${CUDAToolkit_ROOT_DIR}") + # Now that we have the real ROOT_DIR, find components inside it. + list(APPEND CMAKE_PREFIX_PATH ${CUDAToolkit_ROOT_DIR}) + # Mark that we need to pop the prefix path changes after we have + # found the cudart library. + set(_CUDAToolkit_Pop_Prefix True) +endif() +# Find the include/ directory +find_path(CUDAToolkit_INCLUDE_DIR + NAMES cuda_runtime.h + ) +# And find the CUDA Runtime Library libcudart +find_library(CUDA_CUDART + NAMES cudart + PATH_SUFFIXES lib64 lib64/stubs lib/x64 + ) +if (NOT CUDA_CUDART AND NOT CUDAToolkit_FIND_QUIETLY) + message(STATUS "Unable to find cudart library.") +endif() +unset(CUDAToolkit_ROOT_DIR) +if(_CUDAToolkit_Pop_Prefix) + list(REMOVE_AT CMAKE_PREFIX_PATH -1) + unset(_CUDAToolkit_Pop_Prefix) +endif() +#----------------------------------------------------------------------------- +# Perform version comparison and validate all required variables are set. +#include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CUDAToolkit + REQUIRED_VARS + CUDAToolkit_INCLUDE_DIR + CUDA_CUDART + CUDAToolkit_NVCC_EXECUTABLE + VERSION_VAR + CUDAToolkit_VERSION + ) +#----------------------------------------------------------------------------- +# Construct result variables +if(CUDAToolkit_FOUND) + set(CUDAToolkit_INCLUDE_DIRS ${CUDAToolkit_INCLUDE_DIR}) + get_filename_component(CUDAToolkit_LIBRARY_DIR ${CUDA_CUDART} DIRECTORY ABSOLUTE) +endif() +#----------------------------------------------------------------------------- +# Construct import targets +if(CUDAToolkit_FOUND) + function(_CUDAToolkit_find_and_add_import_lib lib_name) + cmake_parse_arguments(arg "" "" "ALT;DEPS;EXTRA_PATH_SUFFIXES" ${ARGN}) + set(search_names ${lib_name} ${arg_ALT}) + # message(STATUS "arg_EXTRA_PATH_SUFFIXES: ${arg_EXTRA_PATH_SUFFIXES}") + find_library(CUDA_${lib_name}_LIBRARY + NAMES ${search_names} + HINTS ${CUDAToolkit_LIBRARY_DIR} + ENV CUDA_PATH + PATH_SUFFIXES nvidia/current lib64 lib64/stubs lib/x64 lib lib/stubs + ${arg_EXTRA_PATH_SUFFIXES} + ) + if (NOT TARGET CUDA::${lib_name} AND CUDA_${lib_name}_LIBRARY) + add_library(CUDA::${lib_name} IMPORTED INTERFACE) + target_include_directories(CUDA::${lib_name} SYSTEM INTERFACE "${CUDAToolkit_INCLUDE_DIRS}") + target_link_libraries(CUDA::${lib_name} INTERFACE "${CUDA_${lib_name}_LIBRARY}") + foreach(dep ${arg_DEPS}) + if(TARGET CUDA::${dep}) + target_link_libraries(CUDA::${lib_name} INTERFACE CUDA::${dep}) + endif() + endforeach() + endif() + endfunction() + if(NOT TARGET CUDA::toolkit) + add_library(CUDA::toolkit IMPORTED INTERFACE) + target_include_directories(CUDA::toolkit SYSTEM INTERFACE "${CUDAToolkit_INCLUDE_DIRS}") + target_link_directories(CUDA::toolkit INTERFACE "${CUDAToolkit_LIBRARY_DIR}") + endif() + _CUDAToolkit_find_and_add_import_lib(cuda_driver ALT cuda) + _CUDAToolkit_find_and_add_import_lib(cudart) + _CUDAToolkit_find_and_add_import_lib(cudart_static) + # setup dependencies that are required for cudart_static when building + # on linux. These are generally only required when using the CUDA toolkit + # when CUDA language is disabled + if(NOT TARGET CUDA::cudart_static_deps + AND TARGET CUDA::cudart_static) + add_library(CUDA::cudart_static_deps IMPORTED INTERFACE) + target_link_libraries(CUDA::cudart_static INTERFACE CUDA::cudart_static_deps) + if(UNIX AND (CMAKE_C_COMPILER OR CMAKE_CXX_COMPILER)) + find_package(Threads REQUIRED) + target_link_libraries(CUDA::cudart_static_deps INTERFACE Threads::Threads ${CMAKE_DL_LIBS}) + endif() + if(UNIX AND NOT APPLE) + # On Linux, you must link against librt when using the static cuda runtime. + find_library(CUDAToolkit_rt_LIBRARY rt) + if(NOT CUDAToolkit_rt_LIBRARY) + message(WARNING "Could not find librt library, needed by CUDA::cudart_static") + else() + target_link_libraries(CUDA::cudart_static_deps INTERFACE ${CUDAToolkit_rt_LIBRARY}) + endif() + endif() + endif() + _CUDAToolkit_find_and_add_import_lib(culibos) # it's a static library + foreach (cuda_lib cublas cufft curand cusparse nppc nvjpeg) + _CUDAToolkit_find_and_add_import_lib(${cuda_lib}) + _CUDAToolkit_find_and_add_import_lib(${cuda_lib}_static DEPS culibos) + endforeach() + # cuFFTW depends on cuFFT + _CUDAToolkit_find_and_add_import_lib(cufftw DEPS cufft) + _CUDAToolkit_find_and_add_import_lib(cufftw DEPS cufft_static) + # cuSOLVER depends on cuBLAS, and cuSPARSE + _CUDAToolkit_find_and_add_import_lib(cusolver DEPS cublas cusparse) + _CUDAToolkit_find_and_add_import_lib(cusolver_static DEPS cublas_static cusparse_static culibos) + # nvGRAPH depends on cuRAND, and cuSOLVER. + _CUDAToolkit_find_and_add_import_lib(nvgraph DEPS curand cusolver) + _CUDAToolkit_find_and_add_import_lib(nvgraph_static DEPS curand_static cusolver_static) + # Process the majority of the NPP libraries. + foreach (cuda_lib nppial nppicc nppidei nppif nppig nppim nppist nppitc npps nppicom nppisu) + _CUDAToolkit_find_and_add_import_lib(${cuda_lib} DEPS nppc) + _CUDAToolkit_find_and_add_import_lib(${cuda_lib}_static DEPS nppc_static) + endforeach() + _CUDAToolkit_find_and_add_import_lib(cupti + EXTRA_PATH_SUFFIXES ../extras/CUPTI/lib64/ + ../extras/CUPTI/lib/) + _CUDAToolkit_find_and_add_import_lib(cupti_static + EXTRA_PATH_SUFFIXES ../extras/CUPTI/lib64/ + ../extras/CUPTI/lib/) + _CUDAToolkit_find_and_add_import_lib(nvrtc DEPS cuda_driver) + _CUDAToolkit_find_and_add_import_lib(nvml ALT nvidia-ml nvml) + if(WIN32) + # nvtools can be installed outside the CUDA toolkit directory + # so prefer the NVTOOLSEXT_PATH windows only environment variable + # In addition on windows the most common name is nvToolsExt64_1 + find_library(CUDA_nvToolsExt_LIBRARY + NAMES nvToolsExt64_1 nvToolsExt64 nvToolsExt + PATHS ENV NVTOOLSEXT_PATH + ENV CUDA_PATH + PATH_SUFFIXES lib/x64 lib + ) + endif() + _CUDAToolkit_find_and_add_import_lib(nvToolsExt ALT nvToolsExt64) + _CUDAToolkit_find_and_add_import_lib(OpenCL) +endif() +if(_CUDAToolkit_Pop_ROOT_PATH) + list(REMOVE_AT CMAKE_FIND_ROOT_PATH 0) + unset(_CUDAToolkit_Pop_ROOT_PATH) +endif() \ No newline at end of file diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake index 76fe944f56..ea0bddcc86 100644 --- a/cmake/FindTBB.cmake +++ b/cmake/FindTBB.cmake @@ -99,7 +99,7 @@ if(NOT TBB_FOUND) ################################## if(NOT DEFINED TBB_USE_DEBUG_BUILD) - if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug|RelWithDebInfo|RELWITHDEBINFO|relwithdebinfo)") + if(CMAKE_BUILD_TYPE MATCHES "(Debug|DEBUG|debug)") set(TBB_BUILD_TYPE DEBUG) else() set(TBB_BUILD_TYPE RELEASE) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 5fd8a6f55b..ab91dcbc15 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -135,6 +135,10 @@ if (GTSAM_USE_SUITESPARSE) target_include_directories(gtsam PUBLIC ${CHOLMOD_INCLUDE_DIR}) endif() +if(GTSAM_USE_CUSPARSE) + target_include_directories(gtsam PUBLIC ${CUDAToolkit_INCLUDE_DIRS}) +endif() + # Add includes for source directories 'BEFORE' boost and any system include # paths so that the compiler uses GTSAM headers in our source directory instead # of any previously installed GTSAM headers. diff --git a/gtsam/config.h.in b/gtsam/config.h.in index f14431f08d..9661897140 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -58,6 +58,9 @@ // Whether GTSAM will have the SuiteSparse solver #cmakedefine GTSAM_USE_SUITESPARSE +// Whether GTSAM will have the SuiteSparse GPU solver +#cmakedefine GTSAM_USE_CUSPARSE + // Eigen library version (needed to avoid mixing versions, which often leads // to segfaults) #cmakedefine GTSAM_EIGEN_VERSION_WORLD @GTSAM_EIGEN_VERSION_WORLD@ diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp new file mode 100644 index 0000000000..86efeb2dfb --- /dev/null +++ b/gtsam/linear/CuSparseSolver.cpp @@ -0,0 +1,216 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file CuSparseSolver.cpp + * + * @brief SuiteSparse based linear solver backend for GTSAM + * + * @date Jun 2020 + * @author Fan Jiang + */ + +#include "gtsam/linear/CuSparseSolver.h" + +#ifdef GTSAM_USE_CUSPARSE +#include +#include +#include + +#endif + +#include "gtsam/linear/SparseEigenSolver.h" + +namespace gtsam { + CuSparseSolver::CuSparseSolver(CuSparseSolver::CuSparseSolverType type, + const Ordering &ordering) { + solverType = type; + this->ordering = ordering; + } + + bool CuSparseSolver::isIterative() { + return false; + } + + bool CuSparseSolver::isSequential() { + return false; + } + +#ifdef GTSAM_USE_CUSPARSE + +#define S1(x) #x +#define S2(x) S1(x) +#define ____LOCATION __FILE__ " : " S2(__LINE__) + + void checkCUDAError(cudaError code, const char* location) { + if(code != cudaSuccess) { + throw std::runtime_error(std::string("cudaMalloc error ") + std::to_string(code) + std::string(" at ") + std::string(location)); + } + } + +#define CHECK_CUDA_ERROR(code) checkCUDAError(code, ____LOCATION); + + void checkCuSolverError(cusolverStatus_t code, const char* location) { + if(code != CUSOLVER_STATUS_SUCCESS) { + throw std::runtime_error(std::string("cuSolver error ") + std::to_string(code) + std::string(" at ") + std::string(location)); + } + } + +#define CHECK_CUSOLVER_ERROR(code) checkCuSolverError(code, ____LOCATION); + + void checkCuSparseError(cusparseStatus_t code, const char* location) { + if(code != CUSPARSE_STATUS_SUCCESS) { + throw std::runtime_error(std::string("cuSparse error ") + std::to_string(code) + std::string(" at ") + std::string(location)); + } + } + +#define CHECK_CUSPARSE_ERROR(code) checkCuSparseError(code, ____LOCATION); + + void EigenSparseToCuSparseTranspose( + const Eigen::SparseMatrix &mat, int **row, int **col, double **val) + { + const int num_non0 = mat.nonZeros(); + const int num_outer = mat.cols() + 1; + + cudaError err; + err = cudaMalloc(reinterpret_cast(row), sizeof(int) * num_outer); + if(err != cudaSuccess) { + throw std::runtime_error(std::string("cudaMalloc error: out of memory? trying to allocate ") + std::to_string(err)); + } + if(cudaMalloc(reinterpret_cast(col), sizeof(int) * num_non0) != cudaSuccess) { + cudaFree(row); + throw std::runtime_error("cudaMalloc error: out of memory?"); + } + if(cudaMalloc(reinterpret_cast(val), sizeof(double) * num_non0) != cudaSuccess) { + cudaFree(row); + cudaFree(col); + throw std::runtime_error("cudaMalloc error: out of memory?"); + } + + CHECK_CUDA_ERROR(cudaMemcpy( + *val, mat.valuePtr(), + num_non0 * sizeof(double), + cudaMemcpyHostToDevice)); + CHECK_CUDA_ERROR(cudaMemcpy(*row, mat.outerIndexPtr(), + num_outer * sizeof(int), + cudaMemcpyHostToDevice)); + CHECK_CUDA_ERROR(cudaMemcpy( + *col, mat.innerIndexPtr(), + num_non0 * sizeof(int), + cudaMemcpyHostToDevice)); + } + + VectorValues CuSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + if (solverType == QR) { + throw std::invalid_argument("This solver does not support QR."); + } else if (solverType == CHOLESKY) { + gttic_(CuSparseSolver_optimizeEigenCholesky); + Eigen::SparseMatrix + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); + auto rows = Ab.rows(), cols = Ab.cols(); + Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); + auto At = A.transpose(); + Matrix b = At * Ab.col(cols - 1); + + Eigen::SparseMatrix + AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); + AtA.makeCompressed(); + + gttic_(CuSparseSolver_optimizeEigenCholesky_solve); + + cusolverSpHandle_t solverHandle; + CHECK_CUSOLVER_ERROR(cusolverSpCreate(&solverHandle)); + + cusparseMatDescr_t descrA; + CHECK_CUSPARSE_ERROR(cusparseCreateMatDescr(&descrA)); + CHECK_CUSPARSE_ERROR(cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL)); + + int *AtA_row(NULL), *AtA_col(NULL); + double *AtA_val(NULL); + + EigenSparseToCuSparseTranspose(AtA, &AtA_row, &AtA_col, &AtA_val); + + double *x_gpu(NULL), *b_gpu(NULL); + + CHECK_CUDA_ERROR(cudaMalloc(&x_gpu, sizeof(double) * AtA.cols())); + CHECK_CUDA_ERROR(cudaMalloc(&b_gpu, sizeof(double) * AtA.cols())); + + CHECK_CUDA_ERROR(cudaMemcpy(b_gpu, b.data(), + sizeof(double) * AtA.cols(), + cudaMemcpyHostToDevice)); + + int singularity = 0; + const double tol = 0.00001; + + std::cout << "AtA_row: " << AtA_row << ", AtA_col: " << AtA_col << std::endl; + std::cout << "AtA.row: " << AtA.rows() << ", AtA.col: " << AtA.cols() << std::endl; + + // no internal reordering, so only lower part (upper part of CSC) is used + CHECK_CUSOLVER_ERROR(cusolverSpDcsrlsvchol( + solverHandle, AtA.rows(), AtA.nonZeros(), descrA, + AtA_val, AtA_row, AtA_col, b_gpu, tol, 0, x_gpu, &singularity)); + + Vector x; + x.resize(A.cols()); + CHECK_CUDA_ERROR(cudaMemcpy(x.data(), x_gpu, sizeof(double) * A.cols(), + cudaMemcpyDeviceToHost)); + + cudaFree(AtA_val); + cudaFree(AtA_row); + cudaFree(AtA_col); + cudaFree(b_gpu); + cudaFree(x_gpu); + + if (singularity != -1) + throw std::runtime_error(std::string("ILS in CUDA Solver, singularity: ") + std::to_string(singularity)); + + gttoc_(CuSparseSolver_optimizeEigenCholesky_solve); + + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); + } + } + + VectorValues vv; + + std::map columnIndices; + + { + size_t currentColIndex = 0; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; + } + } + + for (const std::pair keyDim : dims) { + vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + } + + return vv; + } + + throw std::exception(); + } +#else + VectorValues CuSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + throw std::invalid_argument("This GTSAM is compiled without Cholmod support"); + } +#endif +} diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h new file mode 100644 index 0000000000..90721d36ec --- /dev/null +++ b/gtsam/linear/CuSparseSolver.h @@ -0,0 +1,56 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SuiteSparseSolver.h + * + * @brief SuiteSparse based linear solver backend for GTSAM + * + * @date Jun 2020 + * @author Fan Jiang + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + + /** + * Eigen SparseSolver based Backend class + */ + class GTSAM_EXPORT CuSparseSolver : public LinearSolver { + public: + + typedef enum { + QR, + CHOLESKY + } CuSparseSolverType; + + + explicit CuSparseSolver(CuSparseSolver::CuSparseSolverType type, const Ordering &ordering); + + bool isIterative() override; + + bool isSequential() override; + + VectorValues solve(const GaussianFactorGraph &gfg) override; + + protected: + + CuSparseSolverType solverType = QR; + + Ordering ordering; + }; +} // namespace gtsam diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index fe87915534..be6975b5fd 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -18,6 +18,7 @@ #include #include #include +#include namespace gtsam { @@ -34,6 +35,9 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( } else if (params.linearSolverType == LinearSolverParams::SUITESPARSE_CHOLESKY) { return boost::shared_ptr(new SuiteSparseSolver( SuiteSparseSolver::SuiteSparseSolverType::CHOLESKY, *params.ordering)); + } else if (params.linearSolverType == LinearSolverParams::CUSPARSE_CHOLESKY) { + return boost::shared_ptr(new CuSparseSolver( + CuSparseSolver::CuSparseSolverType::CHOLESKY, *params.ordering)); } throw std::runtime_error("Invalid parameters passed"); diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index 61fba7c611..a19c741347 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -48,6 +48,8 @@ std::string LinearSolverParams::linearSolverTranslator( return "EIGEN_CHOLESKY"; case SUITESPARSE_CHOLESKY: return "SUITESPARSE_CHOLESKY"; + case CUSPARSE_CHOLESKY: + return "CUSPARSE_CHOLESKY"; default: throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); @@ -75,6 +77,8 @@ LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( return LinearSolverParams::EIGEN_QR; if (linearSolverType == "SUITESPARSE_CHOLESKY") return LinearSolverParams::SUITESPARSE_CHOLESKY; + if (linearSolverType == "CUSPARSE_CHOLESKY") + return LinearSolverParams::CUSPARSE_CHOLESKY; throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index a714e1490a..82ea8b61cc 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -38,6 +38,7 @@ struct GTSAM_EXPORT LinearSolverParams { EIGEN_QR, EIGEN_CHOLESKY, SUITESPARSE_CHOLESKY, + CUSPARSE_CHOLESKY, } LinearSolverType; LinearSolverType linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer @@ -76,6 +77,10 @@ struct GTSAM_EXPORT LinearSolverParams { return (linearSolverType == SUITESPARSE_CHOLESKY); } + inline bool isCuSparseCholesky() const { + return (linearSolverType == CUSPARSE_CHOLESKY); + } + GaussianFactorGraph::Eliminate getEliminationFunction() const { switch (linearSolverType) { case MULTIFRONTAL_CHOLESKY: diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index c9d8842839..c033cbf3de 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -60,7 +60,7 @@ namespace gtsam { gttic_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); // Solve A*x = b using sparse Cholesky from Eigen - Eigen::CholmodSimplicialLDLT + Eigen::CholmodSupernodalLLT , Eigen::Upper> solver; solver.cholmod().nmethods = 1; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0b2ea06a2a..70e844f035 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -167,7 +167,7 @@ VectorValues NonlinearOptimizer::solve( "NonlinearOptimizer::solve: special cg parameter type is not handled " "in LM solver ..."); } - } else if (params.isEigenQR() || params.isEigenCholesky() || params.isSuiteSparseCholesky()) { + } else if (params.isEigenQR() || params.isEigenCholesky() || params.isSuiteSparseCholesky() || params.isCuSparseCholesky()) { LinearSolverParams lsparams; lsparams.ordering = params.ordering; lsparams.linearSolverType = params.linearSolverType; From cc273e634af37696d9d44ad0d94717e27d78cb6c Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 8 Jun 2020 11:04:54 -0400 Subject: [PATCH 58/91] typo --- gtsam/linear/CuSparseSolver.cpp | 5 +---- gtsam/linear/CuSparseSolver.h | 4 ++-- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index 86efeb2dfb..9a22716494 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -12,7 +12,7 @@ /** * @file CuSparseSolver.cpp * - * @brief SuiteSparse based linear solver backend for GTSAM + * @brief cuSparse based linear solver backend for GTSAM * * @date Jun 2020 * @author Fan Jiang @@ -151,9 +151,6 @@ namespace gtsam { int singularity = 0; const double tol = 0.00001; - std::cout << "AtA_row: " << AtA_row << ", AtA_col: " << AtA_col << std::endl; - std::cout << "AtA.row: " << AtA.rows() << ", AtA.col: " << AtA.cols() << std::endl; - // no internal reordering, so only lower part (upper part of CSC) is used CHECK_CUSOLVER_ERROR(cusolverSpDcsrlsvchol( solverHandle, AtA.rows(), AtA.nonZeros(), descrA, diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index 90721d36ec..c1d07f84fb 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -10,9 +10,9 @@ * -------------------------------------------------------------------------- */ /** - * @file SuiteSparseSolver.h + * @file CuSparseSolver.h * - * @brief SuiteSparse based linear solver backend for GTSAM + * @brief CuSparse based linear solver backend for GTSAM * * @date Jun 2020 * @author Fan Jiang From c5d8c26dfc2d879b738ee8d3d2ad9413dc000435 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 8 Jun 2020 11:24:42 -0400 Subject: [PATCH 59/91] Fix CUDA detection --- CMakeLists.txt | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 31765d2b4b..593a721b4c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -282,8 +282,11 @@ else() set(GTSAM_USE_SUITESPARSE 0) endif() -if(GTSAM_WITH_CUSPARSE) - find_package(CUDAToolkit REQUIRED) +############################################################################### +# Find CUDA +find_package(CUDAToolkit) + +if(CUDAToolkit_FOUND AND GTSAM_WITH_CUSPARSE) list(APPEND GTSAM_ADDITIONAL_LIBRARIES CUDA::cusparse CUDA::cusolver) set(GTSAM_USE_CUSPARSE 1) endif() @@ -590,6 +593,13 @@ elseif(SUITESPARSE_FOUND) else() message(STATUS " GTSAM will use SuiteSparse : SuiteSparse not found") endif() +if(GTSAM_WITH_CUSPARSE) + message(STATUS " GTSAM will use cuSparse : Yes") +elseif(CUDAToolkit_FOUND) + message(STATUS " GTSAM will use cuSparse : cuSparse found but GTSAM_WITH_CUSPARSE is disabled") +else() + message(STATUS " GTSAM will use cuSparse : cuSparse not found") +endif() message(STATUS " Default allocator : ${GTSAM_DEFAULT_ALLOCATOR}") if(GTSAM_THROW_CHEIRALITY_EXCEPTION) From 80abeb23194fecfc96ffb57072e3c33b7c83b9a3 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Mon, 8 Jun 2020 17:44:06 -0400 Subject: [PATCH 60/91] Push WIP on doing AtA on GPU --- gtsam/linear/CuSparseSolver.cpp | 63 ++++++++++++++++++++++++++++++--- gtsam/linear/CuSparseSolver.h | 2 +- 2 files changed, 60 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index 9a22716494..feb9642788 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #endif @@ -56,7 +57,7 @@ namespace gtsam { } } -#define CHECK_CUDA_ERROR(code) checkCUDAError(code, ____LOCATION); +#define CHECK_CUDA_ERROR(code) checkCUDAError(code, ____LOCATION) void checkCuSolverError(cusolverStatus_t code, const char* location) { if(code != CUSOLVER_STATUS_SUCCESS) { @@ -64,7 +65,7 @@ namespace gtsam { } } -#define CHECK_CUSOLVER_ERROR(code) checkCuSolverError(code, ____LOCATION); +#define CHECK_CUSOLVER_ERROR(code) checkCuSolverError(code, ____LOCATION) void checkCuSparseError(cusparseStatus_t code, const char* location) { if(code != CUSPARSE_STATUS_SUCCESS) { @@ -72,7 +73,7 @@ namespace gtsam { } } -#define CHECK_CUSPARSE_ERROR(code) checkCuSparseError(code, ____LOCATION); +#define CHECK_CUSPARSE_ERROR(code) checkCuSparseError(code, ____LOCATION) void EigenSparseToCuSparseTranspose( const Eigen::SparseMatrix &mat, int **row, int **col, double **val) @@ -113,10 +114,59 @@ namespace gtsam { throw std::invalid_argument("This solver does not support QR."); } else if (solverType == CHOLESKY) { gttic_(CuSparseSolver_optimizeEigenCholesky); + Eigen::SparseMatrix Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); auto rows = Ab.rows(), cols = Ab.cols(); Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); +// +// // CSC in Eigen, CSR in CUDA, so A becomes At +// int *At_row(NULL), *At_col(NULL); +// double *At_val(NULL); +// +// int At_num_rows = A.cols(); +// int At_num_cols = A.rows(); +// int At_num_nnz = A.nonZeros(); +// +// std::cout << "Base of At: " << A.outerIndexPtr()[0] << std::endl; +// EigenSparseToCuSparseTranspose(A, &At_row, &At_col, &At_val); +// +// cusparseHandle_t cspHandle = NULL; +// cusparseSpMatDescr_t matA, matB, matC; +// void* dBuffer1 = NULL, *dBuffer2 = NULL; +// size_t bufferSize1 = 0, bufferSize2 = 0; +// +// CHECK_CUSPARSE_ERROR( cusparseCreate(&cspHandle) ); +// +// CHECK_CUSPARSE_ERROR(cusparseCreateCsr(&matA, At_num_rows, At_num_cols, At_num_nnz, +// At_row, At_col, At_val, +// CUSPARSE_INDEX_32I, CUSPARSE_INDEX_32I, +// CUSPARSE_INDEX_BASE_ZERO, CUDA_R_64F) ); +// CHECK_CUSPARSE_ERROR(cusparseCreateCsr(&matB, At_num_rows, At_num_cols, At_num_nnz, +// At_row, At_col, At_val, +// CUSPARSE_INDEX_32I, CUSPARSE_INDEX_32I, +// CUSPARSE_INDEX_BASE_ZERO, CUDA_R_64F) ); +// CHECK_CUSPARSE_ERROR(cusparseCreateCsr(&matC, At_num_rows, At_num_rows, 0, +// NULL, NULL, NULL, +// CUSPARSE_INDEX_32I, CUSPARSE_INDEX_32I, +// CUSPARSE_INDEX_BASE_ZERO, CUDA_R_64F) ); +// +// int AtA_num_rows = A.cols(); +// int AtA_num_cols = A.cols(); +// int AtA_num_nnz = A.nonZeros(); +// int *AtA_row(NULL), *AtA_col(NULL); +// double *AtA_val(NULL); +// CHECK_CUDA_ERROR(cudaMalloc(reinterpret_cast(AtA_row), sizeof(int) * (AtA_num_cols + 1))); +// +// int baseC, nnzC = 0; +// // nnzTotalDevHostPtr points to host memory +// int *nnzTotalDevHostPtr = &nnzC; +// +// CHECK_CUSPARSE_ERROR(cusparseSetPointerMode(cspHandle, CUSPARSE_POINTER_MODE_HOST)); + + //-------------------------------------------------------------------------- + // SpGEMM Computation + auto At = A.transpose(); Matrix b = At * Ab.col(cols - 1); @@ -127,9 +177,11 @@ namespace gtsam { gttic_(CuSparseSolver_optimizeEigenCholesky_solve); + // Create the cuSolver object cusolverSpHandle_t solverHandle; CHECK_CUSOLVER_ERROR(cusolverSpCreate(&solverHandle)); + // Create the matrix descriptor cusparseMatDescr_t descrA; CHECK_CUSPARSE_ERROR(cusparseCreateMatDescr(&descrA)); CHECK_CUSPARSE_ERROR(cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL)); @@ -139,6 +191,8 @@ namespace gtsam { EigenSparseToCuSparseTranspose(AtA, &AtA_row, &AtA_col, &AtA_val); +// std::cout << "Base of AtA: " << AtA.outerIndexPtr()[0] << std::endl; + double *x_gpu(NULL), *b_gpu(NULL); CHECK_CUDA_ERROR(cudaMalloc(&x_gpu, sizeof(double) * AtA.cols())); @@ -166,6 +220,7 @@ namespace gtsam { cudaFree(AtA_col); cudaFree(b_gpu); cudaFree(x_gpu); + cusolverSpDestroy(solverHandle); if (singularity != -1) throw std::runtime_error(std::string("ILS in CUDA Solver, singularity: ") + std::to_string(singularity)); @@ -207,7 +262,7 @@ namespace gtsam { } #else VectorValues CuSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { - throw std::invalid_argument("This GTSAM is compiled without Cholmod support"); + throw std::invalid_argument("This GTSAM is compiled without CUDA support"); } #endif } diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index c1d07f84fb..58dc60fdc2 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -28,7 +28,7 @@ namespace gtsam { /** - * Eigen SparseSolver based Backend class + * cuSparse based Backend class */ class GTSAM_EXPORT CuSparseSolver : public LinearSolver { public: From 112be21f7eced810f1b77f2556720fd5807a6208 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 12 Nov 2020 14:34:04 -0500 Subject: [PATCH 61/91] fix comment typos and variable names --- gtsam/linear/LinearSolver.h | 5 ++-- .../linear/tests/testGaussianFactorGraph.cpp | 23 ++++++++++--------- gtsam/linear/tests/testLinearSolver.cpp | 12 +++++----- 3 files changed, 20 insertions(+), 20 deletions(-) diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 415ad9a79e..fb8832acc0 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -45,9 +45,8 @@ class GTSAM_EXPORT LinearSolver { }; /** - * Factor method for generating a LinearSolver from legacy - * NonlinearOptimizerParams - * @param nonlinear optimizer parameters + * Factor method for generating a LinearSolver from LinearSolverParams + * @param params LinearSolverParams linear optimizer parameters * @return pointer to a LinearSolver object */ static boost::shared_ptr fromLinearSolverParams( diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 7861197aad..d45a711029 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -71,18 +71,19 @@ TEST(GaussianFactorGraph, initialization) { /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: - // x1 x2 x3 x4 x3 b + // x1 x2 x3 x4 x5 b // 1 2 3 0 0 4 // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); - const Key x3 = 0, y2 = 1; - // const Symbol x3('x', 5), y2('p', 3); - gfg.add(x3, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(), Vector2(4, 8), model); - gfg.add(x3, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(), y2, - (Matrix(2, 2) << 11, 12, 14, 15.).finished(), Vector2(13, 16), model); + const Key x123 = 0, x45 = 1; + gfg.add(x123, (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(), + Vector2(4, 8), model); + gfg.add(x123, (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(), + x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(), + Vector2(13, 16), model); auto entries = gfg.sparseJacobian(); // Check the triplets size... @@ -107,12 +108,12 @@ TEST(GaussianFactorGraph, sparseJacobian) { 3, 6,26, 4, 6,32).finished(); Matrix expectedMatlab = expectedT.transpose(); - Matrix matlab = gfg.sparseJacobian_(); + Matrix actual = gfg.sparseJacobian_(); - EXPECT(assert_equal(expectedMatlab, matlab)); + EXPECT(assert_equal(expectedMatlab, actual)); // Call sparseJacobian with optional ordering... - auto ordering = Ordering(list_of(y2)(x3)); + auto ordering = Ordering(list_of(x45)(x123)); entries = gfg.sparseJacobian(ordering); // Check the triplets size... EXPECT_LONGS_EQUAL(16, entries.size()); @@ -142,9 +143,9 @@ TEST(GaussianFactorGraph, sparseJacobian) { 3, 6,26, 4, 6,32).finished(); expectedMatlab = expectedT.transpose(); - matlab = gfg.sparseJacobian_(ordering); + actual = gfg.sparseJacobian_(ordering); - EXPECT(assert_equal(expectedMatlab, matlab)); + EXPECT(assert_equal(expectedMatlab, actual)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 13ffc8b00b..a72b5a6340 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -42,7 +42,7 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { /* ************************************************************************* */ TEST(EigenOptimizer, optimizeEigenQR) { - GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues expected; expected.insert(2, Vector2(-0.1, -0.1)); @@ -51,16 +51,16 @@ TEST(EigenOptimizer, optimizeEigenQR) { LinearSolverParams params; params.linearSolverType = LinearSolverParams::EIGEN_QR; - params.ordering = Ordering::Colamd(A); + params.ordering = Ordering::Colamd(gfg); auto solver = LinearSolver::fromLinearSolverParams(params); - VectorValues actual = solver->solve(A); + VectorValues actual = solver->solve(gfg); EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */ TEST(EigenOptimizer, optimizeEigenCholesky) { - GaussianFactorGraph A = createSimpleGaussianFactorGraph(); + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues expected; expected.insert(2, Vector2(-0.1, -0.1)); @@ -69,10 +69,10 @@ TEST(EigenOptimizer, optimizeEigenCholesky) { LinearSolverParams params; params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; - params.ordering = Ordering::Colamd(A); + params.ordering = Ordering::Colamd(gfg); auto solver = LinearSolver::fromLinearSolverParams(params); - VectorValues actual = solver->solve(A); + VectorValues actual = solver->solve(gfg); EXPECT(assert_equal(expected, actual)); } From d6dfee808d7978c5dbb490173c477dbe39166bd9 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 12 Nov 2020 17:03:58 -0500 Subject: [PATCH 62/91] fix CUDA runtime cmake link --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 593a721b4c..4a92fa322f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -287,7 +287,7 @@ endif() find_package(CUDAToolkit) if(CUDAToolkit_FOUND AND GTSAM_WITH_CUSPARSE) - list(APPEND GTSAM_ADDITIONAL_LIBRARIES CUDA::cusparse CUDA::cusolver) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES CUDA::cusparse CUDA::cusolver CUDA::cudart) set(GTSAM_USE_CUSPARSE 1) endif() From 083544a11a14d380f095d5509230e7ffcede4992 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 12 Nov 2020 17:07:48 -0500 Subject: [PATCH 63/91] add wrapper for elimination-based linear solvers --- gtsam/linear/EliminationSolver.cpp | 61 ++++++++++++++++++++++++ gtsam/linear/EliminationSolver.h | 55 +++++++++++++++++++++ gtsam/linear/LinearSolver.cpp | 37 +++++++++------ gtsam/linear/LinearSolver.h | 7 +++ gtsam/linear/SuiteSparseSolver.cpp | 3 +- gtsam/linear/tests/testLinearSolver.cpp | 63 +++++++++++++++++++------ 6 files changed, 196 insertions(+), 30 deletions(-) create mode 100644 gtsam/linear/EliminationSolver.cpp create mode 100644 gtsam/linear/EliminationSolver.h diff --git a/gtsam/linear/EliminationSolver.cpp b/gtsam/linear/EliminationSolver.cpp new file mode 100644 index 0000000000..4ff8a5f723 --- /dev/null +++ b/gtsam/linear/EliminationSolver.cpp @@ -0,0 +1,61 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EliminationSolver.cpp + * + * @brief Variable elimination based linear solver backend wrapper for GTSAM. + * This class is just a wrapper for factor graph elimination methods to follow + * the "LinearSolver" unified API. + * + * @date Nov 2020 + * @author Gerry Chen + */ + +#include +#include + +namespace gtsam { + +VectorValues EliminationSolver::solve(const GaussianFactorGraph &gfg) { + switch (params_.linearSolverType) { + case LinearSolverParams::MULTIFRONTAL_QR: + return params_.ordering ? gfg.optimize(*params_.ordering, EliminateQR) + : gfg.optimize(EliminateQR); + case LinearSolverParams::MULTIFRONTAL_CHOLESKY: + return params_.ordering + ? gfg.optimize(*params_.ordering, EliminatePreferCholesky) + : gfg.optimize(EliminatePreferCholesky); + case LinearSolverParams::SEQUENTIAL_QR: + return params_.ordering + ? gfg.eliminateSequential(*params_.ordering, EliminateQR, + boost::none, params_.orderingType) + ->optimize() + : gfg.eliminateSequential(EliminateQR, boost::none, + params_.orderingType) + ->optimize(); + case LinearSolverParams::SEQUENTIAL_CHOLESKY: + return params_.ordering + ? gfg.eliminateSequential(*params_.ordering, + EliminatePreferCholesky, boost::none, + params_.orderingType) + ->optimize() + : gfg.eliminateSequential(EliminatePreferCholesky, boost::none, + params_.orderingType) + ->optimize(); + default: + throw std::runtime_error( + "EliminationSolver::solve: Solver type is invalid for " + "EliminationSolver"); + } +}; + +} // namespace gtsam diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h new file mode 100644 index 0000000000..2caf9f472f --- /dev/null +++ b/gtsam/linear/EliminationSolver.h @@ -0,0 +1,55 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file EliminationSolver.h + * + * @brief Variable elimination based linear solver backend wrapper for GTSAM. + * This class is just a wrapper for factor graph elimination methods to follow + * the "LinearSolver" unified API. + * + * @date Nov 2020 + * @author Gerry Chen + */ + +#pragma once + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * variable elimination based linear solver wrapper class + */ +class GTSAM_EXPORT EliminationSolver : public LinearSolver { + public: + explicit EliminationSolver(const LinearSolverParams ¶ms) + : params_(params){}; + + bool isIterative() override { return false; }; + + bool isSequential() override { + return params_.linearSolverType == LinearSolverParams::SEQUENTIAL_QR || + params_.linearSolverType == LinearSolverParams::SEQUENTIAL_CHOLESKY; + }; + + VectorValues solve(const GaussianFactorGraph &gfg) override; + + protected: + LinearSolverParams params_; +}; + +} // namespace gtsam diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index be6975b5fd..e42cf4c888 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -15,6 +15,7 @@ * @author Fan Jiang */ +#include #include #include #include @@ -26,20 +27,28 @@ LinearSolver::LinearSolver() = default; boost::shared_ptr LinearSolver::fromLinearSolverParams( const LinearSolverParams ¶ms) { - if (params.linearSolverType == LinearSolverParams::EIGEN_QR) { - return boost::shared_ptr(new SparseEigenSolver( - SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); - } else if (params.linearSolverType == LinearSolverParams::EIGEN_CHOLESKY) { - return boost::shared_ptr(new SparseEigenSolver( - SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering)); - } else if (params.linearSolverType == LinearSolverParams::SUITESPARSE_CHOLESKY) { - return boost::shared_ptr(new SuiteSparseSolver( - SuiteSparseSolver::SuiteSparseSolverType::CHOLESKY, *params.ordering)); - } else if (params.linearSolverType == LinearSolverParams::CUSPARSE_CHOLESKY) { - return boost::shared_ptr(new CuSparseSolver( - CuSparseSolver::CuSparseSolverType::CHOLESKY, *params.ordering)); + switch (params.linearSolverType) { + case LinearSolverParams::MULTIFRONTAL_QR: + case LinearSolverParams::MULTIFRONTAL_CHOLESKY: + case LinearSolverParams::SEQUENTIAL_QR: + case LinearSolverParams::SEQUENTIAL_CHOLESKY: + return boost::make_shared(params); + case LinearSolverParams::EIGEN_QR: + return boost::shared_ptr(new SparseEigenSolver( + SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); + case LinearSolverParams::EIGEN_CHOLESKY: + return boost::shared_ptr(new SparseEigenSolver( + SparseEigenSolver::SparseEigenSolverType::CHOLESKY, + *params.ordering)); + case LinearSolverParams::SUITESPARSE_CHOLESKY: + return boost::shared_ptr(new SuiteSparseSolver( + SuiteSparseSolver::SuiteSparseSolverType::CHOLESKY, + *params.ordering)); + case LinearSolverParams::CUSPARSE_CHOLESKY: + return boost::shared_ptr(new CuSparseSolver( + CuSparseSolver::CuSparseSolverType::CHOLESKY, *params.ordering)); + default: + throw std::runtime_error("Invalid parameters passed"); } - - throw std::runtime_error("Invalid parameters passed"); } } // namespace gtsam diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index fb8832acc0..7ac5c4888e 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -44,6 +44,13 @@ class GTSAM_EXPORT LinearSolver { throw std::runtime_error("BUG_CHECK: Calling solve of the base class!"); }; + /** + * Alias for `solve` + * @param gfg the GFG to be optimized + * @return the optimization result in VectorValues + */ + VectorValues operator()(const GaussianFactorGraph &gfg) { return solve(gfg); } + /** * Factor method for generating a LinearSolver from LinearSolverParams * @param params LinearSolverParams linear optimizer parameters diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index c033cbf3de..c40c1fca9c 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -110,7 +110,8 @@ namespace gtsam { } #else VectorValues SuiteSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { - throw std::invalid_argument("This GTSAM is compiled without Cholmod support"); + throw std::invalid_argument( + "This GTSAM is compiled without SuiteSparse support"); } #endif } diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index a72b5a6340..c02eacfa12 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -41,7 +41,7 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { } /* ************************************************************************* */ -TEST(EigenOptimizer, optimizeEigenQR) { +TEST(LinearOptimizer, solver) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues expected; @@ -50,30 +50,63 @@ TEST(EigenOptimizer, optimizeEigenQR) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.linearSolverType = LinearSolverParams::EIGEN_QR; params.ordering = Ordering::Colamd(gfg); + // these tests are not wrapped in a for loop to enable easier debugging + // multifrontal cholesky + params.linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; auto solver = LinearSolver::fromLinearSolverParams(params); - VectorValues actual = solver->solve(gfg); + VectorValues actual = (*solver)(gfg); + EXPECT(assert_equal(expected, actual)); + actual = solver->solve(gfg); EXPECT(assert_equal(expected, actual)); -} -/* ************************************************************************* */ -TEST(EigenOptimizer, optimizeEigenCholesky) { - GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + // multifrontal qr + params.linearSolverType = LinearSolverParams::MULTIFRONTAL_QR; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); - VectorValues expected; - expected.insert(2, Vector2(-0.1, -0.1)); - expected.insert(0, Vector2(0.1, -0.2)); - expected.insert(1, Vector2(-0.1, 0.1)); + // sequential cholesky + params.linearSolverType = LinearSolverParams::SEQUENTIAL_CHOLESKY; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); - LinearSolverParams params; + // sequential qr + params.linearSolverType = LinearSolverParams::SEQUENTIAL_QR; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + + // // iterative + // params.linearSolverType = LinearSolverParams::Iterative; + // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + // EXPECT(assert_equal(expected, actual)); + + // // cholmod + // params.linearSolverType = LinearSolverParams::CHOLMOD; + // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + // EXPECT(assert_equal(expected, actual)); + + // eigen qr + params.linearSolverType = LinearSolverParams::EIGEN_QR; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + + // eigen cholesky params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; - params.ordering = Ordering::Colamd(gfg); + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); - auto solver = LinearSolver::fromLinearSolverParams(params); - VectorValues actual = solver->solve(gfg); + // suitesparse cholesky + params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + + // cusparse cholesky + // #ifdef GTSAM_USE_CUSPARSE + params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); + // #endif } /* ************************************************************************* */ From d4c3ce1024f265d0bb009f5de7276531fd1b7ccb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 12 Nov 2020 17:26:12 -0500 Subject: [PATCH 64/91] inline elimination solver --- gtsam/linear/EliminationSolver.cpp | 61 ------------------------------ gtsam/linear/EliminationSolver.h | 43 ++++++++++++++++++++- 2 files changed, 41 insertions(+), 63 deletions(-) delete mode 100644 gtsam/linear/EliminationSolver.cpp diff --git a/gtsam/linear/EliminationSolver.cpp b/gtsam/linear/EliminationSolver.cpp deleted file mode 100644 index 4ff8a5f723..0000000000 --- a/gtsam/linear/EliminationSolver.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file EliminationSolver.cpp - * - * @brief Variable elimination based linear solver backend wrapper for GTSAM. - * This class is just a wrapper for factor graph elimination methods to follow - * the "LinearSolver" unified API. - * - * @date Nov 2020 - * @author Gerry Chen - */ - -#include -#include - -namespace gtsam { - -VectorValues EliminationSolver::solve(const GaussianFactorGraph &gfg) { - switch (params_.linearSolverType) { - case LinearSolverParams::MULTIFRONTAL_QR: - return params_.ordering ? gfg.optimize(*params_.ordering, EliminateQR) - : gfg.optimize(EliminateQR); - case LinearSolverParams::MULTIFRONTAL_CHOLESKY: - return params_.ordering - ? gfg.optimize(*params_.ordering, EliminatePreferCholesky) - : gfg.optimize(EliminatePreferCholesky); - case LinearSolverParams::SEQUENTIAL_QR: - return params_.ordering - ? gfg.eliminateSequential(*params_.ordering, EliminateQR, - boost::none, params_.orderingType) - ->optimize() - : gfg.eliminateSequential(EliminateQR, boost::none, - params_.orderingType) - ->optimize(); - case LinearSolverParams::SEQUENTIAL_CHOLESKY: - return params_.ordering - ? gfg.eliminateSequential(*params_.ordering, - EliminatePreferCholesky, boost::none, - params_.orderingType) - ->optimize() - : gfg.eliminateSequential(EliminatePreferCholesky, boost::none, - params_.orderingType) - ->optimize(); - default: - throw std::runtime_error( - "EliminationSolver::solve: Solver type is invalid for " - "EliminationSolver"); - } -}; - -} // namespace gtsam diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h index 2caf9f472f..8b0134d6e7 100644 --- a/gtsam/linear/EliminationSolver.h +++ b/gtsam/linear/EliminationSolver.h @@ -23,6 +23,7 @@ #pragma once #include +#include #include #include #include @@ -32,7 +33,9 @@ namespace gtsam { /** - * variable elimination based linear solver wrapper class + * Variable elimination based linear solver backend wrapper for GTSAM. + * This class is a wrapper for factor graph elimination methods to follow the + * "LinearSolver" unified API. */ class GTSAM_EXPORT EliminationSolver : public LinearSolver { public: @@ -46,7 +49,43 @@ class GTSAM_EXPORT EliminationSolver : public LinearSolver { params_.linearSolverType == LinearSolverParams::SEQUENTIAL_CHOLESKY; }; - VectorValues solve(const GaussianFactorGraph &gfg) override; + /** + * Solve the Gaussian factor graph using variable elimination. + * @param gfg the factor graph to solve + * @returns the solution + */ + VectorValues solve(const GaussianFactorGraph &gfg) override { + switch (params_.linearSolverType) { + case LinearSolverParams::MULTIFRONTAL_QR: + return params_.ordering ? gfg.optimize(*params_.ordering, EliminateQR) + : gfg.optimize(EliminateQR); + case LinearSolverParams::MULTIFRONTAL_CHOLESKY: + return params_.ordering + ? gfg.optimize(*params_.ordering, EliminatePreferCholesky) + : gfg.optimize(EliminatePreferCholesky); + case LinearSolverParams::SEQUENTIAL_QR: + return params_.ordering + ? gfg.eliminateSequential(*params_.ordering, EliminateQR, + boost::none, params_.orderingType) + ->optimize() + : gfg.eliminateSequential(EliminateQR, boost::none, + params_.orderingType) + ->optimize(); + case LinearSolverParams::SEQUENTIAL_CHOLESKY: + return params_.ordering + ? gfg.eliminateSequential(*params_.ordering, + EliminatePreferCholesky, + boost::none, params_.orderingType) + ->optimize() + : gfg.eliminateSequential(EliminatePreferCholesky, + boost::none, params_.orderingType) + ->optimize(); + default: + throw std::runtime_error( + "EliminationSolver::solve: Solver type is invalid for " + "EliminationSolver"); + } + }; protected: LinearSolverParams params_; From c6a21015fab81f3591e1d6e33a4b9412e7d4e482 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 01:25:56 -0500 Subject: [PATCH 65/91] extend iterative solvers to inherit from LinearSolver --- gtsam/linear/IterativeSolver.cpp | 26 +++++++++++++++ gtsam/linear/IterativeSolver.h | 20 ++++++++++-- gtsam/linear/LinearSolver.cpp | 29 ++++++++++++++++- gtsam/linear/LinearSolverParams.h | 21 ++++++++---- gtsam/linear/PCGSolver.h | 6 ++-- gtsam/linear/SubgraphSolver.h | 24 ++++++++++++++ gtsam/linear/tests/testLinearSolver.cpp | 38 ++++++++++++++++++---- gtsam/nonlinear/NonlinearOptimizerParams.h | 1 - 8 files changed, 145 insertions(+), 20 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index c5007206d2..9f474a7a70 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -19,7 +19,12 @@ #include #include #include +#include +#include +#include + #include + #include using namespace std; @@ -96,6 +101,27 @@ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } +/*****************************************************************************/ +boost::shared_ptr IterativeSolver::fromLinearSolverParams( + const LinearSolverParams ¶ms) { + if (!params.iterativeParams) { + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + } else if (auto pcg = boost::dynamic_pointer_cast( + params.iterativeParams)) { + return boost::make_shared(*pcg); + } else if (auto spcg = boost::dynamic_pointer_cast( + params.iterativeParams)) { + if (!params.ordering) + throw std::runtime_error("SubgraphSolver needs an ordering"); + return boost::make_shared(*spcg, *params.ordering); + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: special cg parameter type is not handled " + "in LM solver ..."); + } +}; + /****************************************************************************/ KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) : ordering_(ordering) { diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 758d2aec97..63a93db482 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include @@ -83,8 +85,8 @@ class IterativeOptimizationParameters { /** * Base class for Iterative Solvers like SubgraphSolver */ -class IterativeSolver { -public: +class IterativeSolver : public LinearSolver { + public: typedef boost::shared_ptr shared_ptr; IterativeSolver() { } @@ -105,6 +107,20 @@ class IterativeSolver { const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial) = 0; + /* satisfies LinearSolver interface */ + bool isIterative() override { return true; }; + + /* satisfies LinearSolver interface */ + bool isSequential() override { return false; }; + + /* satisfies LinearSolver interface */ + VectorValues solve(const GaussianFactorGraph &gfg) override { + return optimize(gfg); + }; + + /* constructs PCGSolver or SubgraphSolver pointer */ + static boost::shared_ptr fromLinearSolverParams( + const LinearSolverParams ¶ms); }; /** diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index e42cf4c888..4eae23eb22 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -15,8 +15,11 @@ * @author Fan Jiang */ -#include #include +#include +#include +#include +#include #include #include #include @@ -33,6 +36,29 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( case LinearSolverParams::SEQUENTIAL_QR: case LinearSolverParams::SEQUENTIAL_CHOLESKY: return boost::make_shared(params); + case LinearSolverParams::Iterative: + return IterativeSolver::fromLinearSolverParams(params); + case LinearSolverParams::PCG: + if (!params.iterativeParams) + throw std::runtime_error( + "LinearSolver::fromLinearSolverParams: iterative params has to be " + "assigned ..."); + return boost::make_shared( + *boost::static_pointer_cast( + params.iterativeParams)); + case LinearSolverParams::SUBGRAPH: + if (!params.iterativeParams) + throw std::runtime_error( + "LinearSolver::fromLinearSolverParams: iterative params has to be " + "assigned ..."); + if (!params.ordering) + throw std::runtime_error( + "LinearSolver::fromLinearSolverParams: SubgraphSolver needs an " + "ordering"); + return boost::make_shared( + *boost::static_pointer_cast( + params.iterativeParams), + *params.ordering); case LinearSolverParams::EIGEN_QR: return boost::shared_ptr(new SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); @@ -47,6 +73,7 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( case LinearSolverParams::CUSPARSE_CHOLESKY: return boost::shared_ptr(new CuSparseSolver( CuSparseSolver::CuSparseSolverType::CHOLESKY, *params.ordering)); + case LinearSolverParams::CHOLMOD: default: throw std::runtime_error("Invalid parameters passed"); } diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 82ea8b61cc..a8a038204c 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -19,22 +19,29 @@ #include #include -#include #include namespace gtsam { +// forward declaration +class IterativeOptimizationParameters; + struct GTSAM_EXPORT LinearSolverParams { -public: + public: // Type of solver typedef enum LinearSolverType { MULTIFRONTAL_CHOLESKY, MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - Iterative, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ + Iterative, /* Experimental Flag - donotuse: for backwards compatibility + only. Use PCG or SUBGRAPH instead */ + CHOLMOD, /* Experimental Flag - donotuse: for backwards compatibility. use + SUITESPARSE_CHOLESKY or PCG/SUBGRAPH w/ iterativeParams instead + */ + PCG, + SUBGRAPH, EIGEN_QR, EIGEN_CHOLESKY, SUITESPARSE_CHOLESKY, @@ -45,11 +52,11 @@ struct GTSAM_EXPORT LinearSolverParams { Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + boost::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) - || (linearSolverType == MULTIFRONTAL_QR); + return (linearSolverType == MULTIFRONTAL_CHOLESKY) || + (linearSolverType == MULTIFRONTAL_QR); } inline bool isSequential() const { diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index f5b278ae51..f02710d536 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -38,8 +38,10 @@ struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; - PCGSolverParameters() { - } + PCGSolverParameters() {} + PCGSolverParameters( + boost::shared_ptr preconditioner) + : preconditioner_(preconditioner){}; virtual void print(std::ostream &os) const; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 5ab1a8520e..129d5f4aae 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include // pair @@ -155,4 +157,26 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { #endif }; +/** + * This class is a wrapper around SubgraphSolver to more cleanly satisfy the + * LinearSolver interface. Specifically, rather than partitioning the + * subgraph during construction, instead the partitioning will occur during + * "solve" since the GaussianFactorGraph is needed to partition the graph. + */ +class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { + public: + SubgraphSolverWrapper(const SubgraphSolverParameters ¶meters, + const Ordering &ordering) + : parameters_(parameters), ordering_(ordering) {}; + + /// satisfies LinearSolver interface to solve the GaussianFactorGraph. + VectorValues solve(const GaussianFactorGraph &gfg) override { + return SubgraphSolver(gfg, parameters_, ordering_).optimize(); + }; + + protected: + SubgraphSolverParameters parameters_; + Ordering ordering_; +}; + } // namespace gtsam diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index c02eacfa12..327a577467 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -16,6 +16,9 @@ */ #include +#include +#include +#include #include #include #include @@ -76,16 +79,35 @@ TEST(LinearOptimizer, solver) { actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // // iterative - // params.linearSolverType = LinearSolverParams::Iterative; - // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); - // EXPECT(assert_equal(expected, actual)); + // iterative + params.linearSolverType = LinearSolverParams::Iterative; + params.iterativeParams = boost::make_shared( + boost::make_shared()); + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + params.iterativeParams = boost::make_shared(); + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); - // // cholmod + // // cholmod - this flag exists for backwards compatibility but doesn't really + // // correspond to any meaningful action // params.linearSolverType = LinearSolverParams::CHOLMOD; // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); // EXPECT(assert_equal(expected, actual)); + // PCG + params.linearSolverType = LinearSolverParams::PCG; + params.iterativeParams = boost::make_shared( + boost::make_shared()); + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + + // Subgraph + params.linearSolverType = LinearSolverParams::SUBGRAPH; + params.iterativeParams = boost::make_shared(); + actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + // eigen qr params.linearSolverType = LinearSolverParams::EIGEN_QR; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); @@ -97,16 +119,18 @@ TEST(LinearOptimizer, solver) { EXPECT(assert_equal(expected, actual)); // suitesparse cholesky + #ifdef GTSAM_USE_SUITESPARSE params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); + #endif // cusparse cholesky - // #ifdef GTSAM_USE_CUSPARSE + #ifdef GTSAM_USE_CUSPARSE params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // #endif + #endif } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 28759d9466..3d8529eeeb 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -22,7 +22,6 @@ #pragma once #include -#include #include #include #include From e27d02b85d894c2ccc9d94a5764a5e406b3add60 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 01:57:41 -0500 Subject: [PATCH 66/91] test Nonlinear Solver with different linear backends --- gtsam/linear/LinearSolverParams.h | 4 ++- gtsam/linear/tests/testLinearSolver.cpp | 26 ++++++++-------- tests/testNonlinearOptimizer.cpp | 41 +++++++++++++------------ 3 files changed, 38 insertions(+), 33 deletions(-) diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index a8a038204c..ab6e4dc106 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -46,6 +46,7 @@ struct GTSAM_EXPORT LinearSolverParams { EIGEN_CHOLESKY, SUITESPARSE_CHOLESKY, CUSPARSE_CHOLESKY, + LAST /* for iterating over enum */ } LinearSolverType; LinearSolverType linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer @@ -69,7 +70,8 @@ struct GTSAM_EXPORT LinearSolverParams { } inline bool isIterative() const { - return (linearSolverType == Iterative); + return (linearSolverType == Iterative) || (linearSolverType == PCG) || + (linearSolverType == SUBGRAPH); } inline bool isEigenQR() const { diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 327a577467..7731f2ffaf 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -55,8 +55,10 @@ TEST(LinearOptimizer, solver) { LinearSolverParams params; params.ordering = Ordering::Colamd(gfg); - // these tests are not wrapped in a for loop to enable easier debugging - // multifrontal cholesky + // Below we solve with different backend linear solver choices + // Note: these tests are not in a for loop to enable easier debugging + + // Multifrontal Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; auto solver = LinearSolver::fromLinearSolverParams(params); VectorValues actual = (*solver)(gfg); @@ -64,22 +66,22 @@ TEST(LinearOptimizer, solver) { actual = solver->solve(gfg); EXPECT(assert_equal(expected, actual)); - // multifrontal qr + // Multifrontal QR, will be parallel if TBB installed params.linearSolverType = LinearSolverParams::MULTIFRONTAL_QR; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // sequential cholesky + // Sequential Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::SEQUENTIAL_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // sequential qr + // Sequential QR, not parallelized params.linearSolverType = LinearSolverParams::SEQUENTIAL_QR; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // iterative + // Iterative - either PCGSolver or SubgraphSolver params.linearSolverType = LinearSolverParams::Iterative; params.iterativeParams = boost::make_shared( boost::make_shared()); @@ -95,37 +97,37 @@ TEST(LinearOptimizer, solver) { // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); // EXPECT(assert_equal(expected, actual)); - // PCG + // PCG - Preconditioned Conjugate Gradient, an iterative method params.linearSolverType = LinearSolverParams::PCG; params.iterativeParams = boost::make_shared( boost::make_shared()); actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // Subgraph + // Subgraph - SPCG, see SubgraphSolver.h params.linearSolverType = LinearSolverParams::SUBGRAPH; params.iterativeParams = boost::make_shared(); actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // eigen qr + // Sparse Eigen QR params.linearSolverType = LinearSolverParams::EIGEN_QR; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // eigen cholesky + // Sparse Eigen Cholesky params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // suitesparse cholesky + // SuiteSparse Cholesky #ifdef GTSAM_USE_SUITESPARSE params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif - // cusparse cholesky + // CuSparse Cholesky #ifdef GTSAM_USE_CUSPARSE params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index bc6f61f333..20edec10c0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -25,6 +25,10 @@ #include #include #include +#include +#include +#include +#include #include #include #include @@ -161,28 +165,25 @@ TEST(NonlinearOptimizer, optimization_method) { Values c0; c0.insert(X(1), x0); - // Below we solve with different backend linear solver choices LevenbergMarquardtParams params; - // Multifrontal QR, will be parallel if TBB installed - params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_QR; - Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0, fg.error(actualMFQR), tol); - - // Multifrontal Cholesky (more sensitive to conditioning, but faster) - params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; - Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0, fg.error(actualMFChol), tol); - - // Test sparse Eigen QR solver - params.linearSolverType = NonlinearOptimizerParams::EIGEN_QR; - Values actualEigenQR = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0, fg.error(actualEigenQR), tol); - - // Test sparse Eigen Cholesky solver - params.linearSolverType = NonlinearOptimizerParams::EIGEN_CHOLESKY; - Values actualEigenCholesky = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); - DOUBLES_EQUAL(0, fg.error(actualEigenCholesky), tol); + // Test all linear solvers + typedef LinearSolverParams LSP; + for (int solver = LSP::MULTIFRONTAL_CHOLESKY; + solver != LSP::LAST; solver++) { + if (solver == LSP::CHOLMOD) continue; + params.linearSolverType = + static_cast(solver); + params.iterativeParams = + (solver == LSP::Iterative) || (solver == LSP::PCG) + ? boost::make_shared( + boost::make_shared()) + : (solver == LSP::SUBGRAPH) + ? boost::make_shared() + : boost::make_shared(); + Values actual = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); + DOUBLES_EQUAL(0, fg.error(actual), tol); + } } /* ************************************************************************* */ From 76ba028361693ace2501ad1aad6c1d219410b1a7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 02:00:41 -0500 Subject: [PATCH 67/91] nonlinear solver linear solve - use LinearSolver --- gtsam/nonlinear/NonlinearOptimizer.cpp | 51 +------------------------- 1 file changed, 1 insertion(+), 50 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 70e844f035..ce4b7d37b3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -130,56 +130,7 @@ VectorValues NonlinearOptimizer::solve( const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point - VectorValues delta; - - // Check which solver we are using - if (params.isMultifrontal()) { - // Multifrontal QR or Cholesky (decided by params.getEliminationFunction()) - if (params.ordering) - delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); - else - delta = gfg.optimize(params.getEliminationFunction()); - } else if (params.isSequential()) { - // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - if (params.ordering) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), - boost::none, params.orderingType)->optimize(); - else - delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); - } else if (params.isIterative()) { - // Conjugate Gradient -> needs params.iterativeParams - if (!params.iterativeParams) - throw std::runtime_error( - "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - - if (auto pcg = boost::dynamic_pointer_cast( - params.iterativeParams)) { - delta = PCGSolver(*pcg).optimize(gfg); - } else if (auto spcg = - boost::dynamic_pointer_cast( - params.iterativeParams)) { - if (!params.ordering) - throw std::runtime_error("SubgraphSolver needs an ordering"); - delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); - } else { - throw std::runtime_error( - "NonlinearOptimizer::solve: special cg parameter type is not handled " - "in LM solver ..."); - } - } else if (params.isEigenQR() || params.isEigenCholesky() || params.isSuiteSparseCholesky() || params.isCuSparseCholesky()) { - LinearSolverParams lsparams; - lsparams.ordering = params.ordering; - lsparams.linearSolverType = params.linearSolverType; - auto solver = LinearSolver::fromLinearSolverParams(lsparams); - delta = solver->solve(gfg); - } else { - throw std::runtime_error( - "NonlinearOptimizer::solve: Optimization parameter is invalid"); - } - - // return update - return delta; + return LinearSolver::fromLinearSolverParams(params)->solve(gfg); } /* ************************************************************************* */ From 832391a34232ad721ca0b5b1831fbc8bc3a5d4ae Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 06:25:59 -0500 Subject: [PATCH 68/91] NonlinearOptimizerParams no longer inherits from LinearSolverParams --- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 93 ++++++++++++++++++++-- tests/testNonlinearOptimizer.cpp | 3 +- 3 files changed, 90 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index ce4b7d37b3..46bf8103cb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -130,7 +130,7 @@ VectorValues NonlinearOptimizer::solve( const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point - return LinearSolver::fromLinearSolverParams(params)->solve(gfg); + return LinearSolver::fromLinearSolverParams(params.linearSolverParams)->solve(gfg); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 3d8529eeeb..8fd393451c 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -29,11 +29,14 @@ namespace gtsam { +// forward declaration +class IterativeOptimizationParameters; + /** The common parameters for Nonlinear optimizers. Most optimizers * deriving from NonlinearOptimizer also subclass the parameters. */ -class GTSAM_EXPORT NonlinearOptimizerParams: public LinearSolverParams { -public: +class GTSAM_EXPORT NonlinearOptimizerParams { + public: /** See NonlinearOptimizerParams::verbosity */ enum Verbosity { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR @@ -45,9 +48,12 @@ class GTSAM_EXPORT NonlinearOptimizerParams: public LinearSolverParams { double errorTol; ///< The maximum total error to stop iterating (default 0.0) Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( - 0.0), verbosity(SILENT) {} + NonlinearOptimizerParams() + : maxIterations(100), + relativeErrorTol(1e-5), + absoluteErrorTol(1e-5), + errorTol(0.0), + verbosity(SILENT) {} virtual ~NonlinearOptimizerParams() { } @@ -69,6 +75,83 @@ class GTSAM_EXPORT NonlinearOptimizerParams: public LinearSolverParams { static Verbosity verbosityTranslator(const std::string &s) ; static std::string verbosityTranslator(Verbosity value) ; + + /// The parameters for the linear backend solver + LinearSolverParams linearSolverParams; + + /** + * @name Linear Properties for Backwards Compatibility + * These member variables and functions reference LinearSolverParams + */ + ///@{ + + /** See LinearSolverParams::LinearSolverType */ + typedef LinearSolverParams::LinearSolverType LinearSolverType; + static constexpr LinearSolverType MULTIFRONTAL_CHOLESKY = LinearSolverParams::MULTIFRONTAL_CHOLESKY; + static constexpr LinearSolverType MULTIFRONTAL_QR = LinearSolverParams::MULTIFRONTAL_QR; + static constexpr LinearSolverType SEQUENTIAL_CHOLESKY = LinearSolverParams::SEQUENTIAL_CHOLESKY; + static constexpr LinearSolverType SEQUENTIAL_QR = LinearSolverParams::SEQUENTIAL_QR; + static constexpr LinearSolverType Iterative = LinearSolverParams::Iterative; /* Experimental Flag */ + static constexpr LinearSolverType CHOLMOD = LinearSolverParams::CHOLMOD; /* Experimental Flag */ + static constexpr LinearSolverType PCG = LinearSolverParams::PCG; + static constexpr LinearSolverType SUBGRAPH = LinearSolverParams::SUBGRAPH; + static constexpr LinearSolverType EIGEN_QR = LinearSolverParams::EIGEN_QR; + static constexpr LinearSolverType EIGEN_CHOLESKY = LinearSolverParams::EIGEN_CHOLESKY; + static constexpr LinearSolverType SUITESPARSE_CHOLESKY = LinearSolverParams::SUITESPARSE_CHOLESKY; + static constexpr LinearSolverType CUSPARSE_CHOLESKY = LinearSolverParams::CUSPARSE_CHOLESKY; + static constexpr LinearSolverType LAST = LinearSolverParams::LAST; + + LinearSolverType &linearSolverType {linearSolverParams.linearSolverType}; ///< The type of linear solver to use in the nonlinear optimizer + Ordering::OrderingType &orderingType {linearSolverParams.orderingType}; + boost::optional &ordering {linearSolverParams.ordering}; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) + boost::shared_ptr &iterativeParams {linearSolverParams.iterativeParams}; ///< The container for iterativeOptimization parameters. used in CG Solvers. + + inline bool isMultifrontal() const { + return linearSolverParams.isMultifrontal(); + } + + inline bool isSequential() const { + return linearSolverParams.isSequential(); + } + + inline bool isCholmod() const { + return linearSolverParams.isCholmod(); + } + + inline bool isIterative() const { + return linearSolverParams.isIterative(); + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return linearSolverParams.getEliminationFunction(); + } + + std::string getLinearSolverType() const { + return linearSolverParams.getLinearSolverType(); + } + + void setLinearSolverType(const std::string& solver) { + linearSolverParams.setLinearSolverType(solver); + } + + void setIterativeParams(const boost::shared_ptr params) { + linearSolverParams.setIterativeParams(params); + } + + void setOrdering(const Ordering& ordering) { + linearSolverParams.setOrdering(ordering); + } + + std::string getOrderingType() const { + return linearSolverParams.getOrderingType(); + } + + // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type + void setOrderingType(const std::string& ordering){ + linearSolverParams.setOrderingType(ordering); + } + + ///@} }; // For backward compatibility: diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 20edec10c0..0a017dab43 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -172,8 +172,7 @@ TEST(NonlinearOptimizer, optimization_method) { for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { if (solver == LSP::CHOLMOD) continue; - params.linearSolverType = - static_cast(solver); + params.linearSolverType = static_cast(solver); params.iterativeParams = (solver == LSP::Iterative) || (solver == LSP::PCG) ? boost::make_shared( From c7805e12717b3a6c6af11913beadfe980e3de348 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 06:28:48 -0500 Subject: [PATCH 69/91] fromLinearSolverParams -> uppercase: FromLinearSolverParams --- gtsam/linear/IterativeSolver.cpp | 2 +- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/LinearSolver.cpp | 10 +++++----- gtsam/linear/LinearSolver.h | 2 +- gtsam/linear/tests/testLinearSolver.cpp | 26 ++++++++++++------------- gtsam/nonlinear/NonlinearOptimizer.cpp | 2 +- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 9f474a7a70..17a7147c7e 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -102,7 +102,7 @@ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, } /*****************************************************************************/ -boost::shared_ptr IterativeSolver::fromLinearSolverParams( +boost::shared_ptr IterativeSolver::FromLinearSolverParams( const LinearSolverParams ¶ms) { if (!params.iterativeParams) { throw std::runtime_error( diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 63a93db482..2f427577db 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -119,7 +119,7 @@ class IterativeSolver : public LinearSolver { }; /* constructs PCGSolver or SubgraphSolver pointer */ - static boost::shared_ptr fromLinearSolverParams( + static boost::shared_ptr FromLinearSolverParams( const LinearSolverParams ¶ms); }; diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 4eae23eb22..3a42b76efe 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -28,7 +28,7 @@ namespace gtsam { LinearSolver::LinearSolver() = default; -boost::shared_ptr LinearSolver::fromLinearSolverParams( +boost::shared_ptr LinearSolver::FromLinearSolverParams( const LinearSolverParams ¶ms) { switch (params.linearSolverType) { case LinearSolverParams::MULTIFRONTAL_QR: @@ -37,11 +37,11 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( case LinearSolverParams::SEQUENTIAL_CHOLESKY: return boost::make_shared(params); case LinearSolverParams::Iterative: - return IterativeSolver::fromLinearSolverParams(params); + return IterativeSolver::FromLinearSolverParams(params); case LinearSolverParams::PCG: if (!params.iterativeParams) throw std::runtime_error( - "LinearSolver::fromLinearSolverParams: iterative params has to be " + "LinearSolver::FromLinearSolverParams: iterative params has to be " "assigned ..."); return boost::make_shared( *boost::static_pointer_cast( @@ -49,11 +49,11 @@ boost::shared_ptr LinearSolver::fromLinearSolverParams( case LinearSolverParams::SUBGRAPH: if (!params.iterativeParams) throw std::runtime_error( - "LinearSolver::fromLinearSolverParams: iterative params has to be " + "LinearSolver::FromLinearSolverParams: iterative params has to be " "assigned ..."); if (!params.ordering) throw std::runtime_error( - "LinearSolver::fromLinearSolverParams: SubgraphSolver needs an " + "LinearSolver::FromLinearSolverParams: SubgraphSolver needs an " "ordering"); return boost::make_shared( *boost::static_pointer_cast( diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 7ac5c4888e..7d90680095 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -56,7 +56,7 @@ class GTSAM_EXPORT LinearSolver { * @param params LinearSolverParams linear optimizer parameters * @return pointer to a LinearSolver object */ - static boost::shared_ptr fromLinearSolverParams( + static boost::shared_ptr FromLinearSolverParams( const LinearSolverParams ¶ms); protected: diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 7731f2ffaf..7c61527a38 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -60,7 +60,7 @@ TEST(LinearOptimizer, solver) { // Multifrontal Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; - auto solver = LinearSolver::fromLinearSolverParams(params); + auto solver = LinearSolver::FromLinearSolverParams(params); VectorValues actual = (*solver)(gfg); EXPECT(assert_equal(expected, actual)); actual = solver->solve(gfg); @@ -68,69 +68,69 @@ TEST(LinearOptimizer, solver) { // Multifrontal QR, will be parallel if TBB installed params.linearSolverType = LinearSolverParams::MULTIFRONTAL_QR; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sequential Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::SEQUENTIAL_CHOLESKY; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sequential QR, not parallelized params.linearSolverType = LinearSolverParams::SEQUENTIAL_QR; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Iterative - either PCGSolver or SubgraphSolver params.linearSolverType = LinearSolverParams::Iterative; params.iterativeParams = boost::make_shared( boost::make_shared()); - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); params.iterativeParams = boost::make_shared(); - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // // cholmod - this flag exists for backwards compatibility but doesn't really // // correspond to any meaningful action // params.linearSolverType = LinearSolverParams::CHOLMOD; - // actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + // actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); // EXPECT(assert_equal(expected, actual)); // PCG - Preconditioned Conjugate Gradient, an iterative method params.linearSolverType = LinearSolverParams::PCG; params.iterativeParams = boost::make_shared( boost::make_shared()); - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Subgraph - SPCG, see SubgraphSolver.h params.linearSolverType = LinearSolverParams::SUBGRAPH; params.iterativeParams = boost::make_shared(); - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sparse Eigen QR params.linearSolverType = LinearSolverParams::EIGEN_QR; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sparse Eigen Cholesky params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // SuiteSparse Cholesky #ifdef GTSAM_USE_SUITESPARSE params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif // CuSparse Cholesky #ifdef GTSAM_USE_CUSPARSE params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; - actual = LinearSolver::fromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 46bf8103cb..81cc7d94b3 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -130,7 +130,7 @@ VectorValues NonlinearOptimizer::solve( const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point - return LinearSolver::fromLinearSolverParams(params.linearSolverParams)->solve(gfg); + return LinearSolver::FromLinearSolverParams(params.linearSolverParams)->solve(gfg); } /* ************************************************************************* */ From 7619643effeec904005f4d91be365e8d158e78cc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 06:40:13 -0500 Subject: [PATCH 70/91] ifdef guards on suitesparse and cusparse compile flags in testNonlinearOptimizer --- tests/testNonlinearOptimizer.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 0a017dab43..44f09064d9 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -155,6 +155,16 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) DOUBLES_EQUAL(0,fg.error(actual),tol); } +IterativeOptimizationParameters::shared_ptr createIterativeParams(int solver) { + typedef LinearSolverParams LSP; + return (solver == LSP::Iterative) || (solver == LSP::PCG) + ? boost::make_shared( + boost::make_shared()) + : (solver == LSP::SUBGRAPH) + ? boost::make_shared() + : boost::make_shared(); +} + /* ************************************************************************* */ TEST(NonlinearOptimizer, optimization_method) { // Create nonlinear example @@ -171,15 +181,15 @@ TEST(NonlinearOptimizer, optimization_method) { typedef LinearSolverParams LSP; for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { - if (solver == LSP::CHOLMOD) continue; + if (solver == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option +#ifndef GTSAM_USE_SUITESPARSE + if (solver == LSP::SUITESPARSE_CHOLESKY) continue; +#endif +#ifndef GTSAM_USE_CUSPARSE + if (solver == LSP::CUSPARSE_CHOLESKY) continue; +#endif params.linearSolverType = static_cast(solver); - params.iterativeParams = - (solver == LSP::Iterative) || (solver == LSP::PCG) - ? boost::make_shared( - boost::make_shared()) - : (solver == LSP::SUBGRAPH) - ? boost::make_shared() - : boost::make_shared(); + params.iterativeParams = createIterativeParams(solver); Values actual = LevenbergMarquardtOptimizer(fg, c0, params).optimize(); DOUBLES_EQUAL(0, fg.error(actual), tol); } From d2b0c804727d33abfd2c348ba22d80dd7b631bad Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 07:17:50 -0500 Subject: [PATCH 71/91] LinearSolverParams constructor and extra unit test --- gtsam/linear/LinearSolverParams.cpp | 8 ++++ gtsam/linear/LinearSolverParams.h | 33 ++++++++++++- gtsam/linear/tests/testLinearSolver.cpp | 64 +++++++++++++++++++++---- tests/testNonlinearOptimizer.cpp | 3 +- 4 files changed, 97 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index a19c741347..ef55bc39ac 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -42,6 +42,10 @@ std::string LinearSolverParams::linearSolverTranslator( return "ITERATIVE"; case CHOLMOD: return "CHOLMOD"; + case PCG: + return "PCG"; + case SUBGRAPH: + return "SUBGRAPH"; case EIGEN_QR: return "EIGEN_QR"; case EIGEN_CHOLESKY: @@ -71,6 +75,10 @@ LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( return LinearSolverParams::Iterative; if (linearSolverType == "CHOLMOD") return LinearSolverParams::CHOLMOD; + if (linearSolverType == "PCG") + return LinearSolverParams::PCG; + if (linearSolverType == "SUBGRAPH") + return LinearSolverParams::SUBGRAPH; if (linearSolverType == "EIGEN_CHOLESKY") return LinearSolverParams::EIGEN_CHOLESKY; if (linearSolverType == "EIGEN_QR") diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index ab6e4dc106..16ae92c71c 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -49,7 +49,38 @@ struct GTSAM_EXPORT LinearSolverParams { LAST /* for iterating over enum */ } LinearSolverType; - LinearSolverType linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer + /// Construct a params object from the solver and ordering types + LinearSolverParams(LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY, + Ordering::OrderingType orderingType = Ordering::COLAMD) + : linearSolverType(linearSolverType), + orderingType(orderingType) {}; + /// Construct a params object from the solver type and custom ordering + LinearSolverParams(LinearSolverType linearSolverType, + boost::optional ordering) + : linearSolverType(linearSolverType), + orderingType(Ordering::CUSTOM), + ordering(ordering) {}; + /// Construct a params object from the solver and ordering types as well as + /// the iterative parameters + LinearSolverParams( + LinearSolverType linearSolverType, + Ordering::OrderingType orderingType, + boost::shared_ptr iterativeParams) + : linearSolverType(linearSolverType), + orderingType(orderingType), + iterativeParams(iterativeParams) {}; + /// Construct a params object from the solver type, custom ordering, and the + /// iterative parameters + LinearSolverParams( + LinearSolverType linearSolverType, + boost::optional ordering, + boost::shared_ptr iterativeParams) + : linearSolverType(linearSolverType), + orderingType(Ordering::CUSTOM), + ordering(ordering), + iterativeParams(iterativeParams) {}; + + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 7c61527a38..736a4f3973 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -44,7 +44,7 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { } /* ************************************************************************* */ -TEST(LinearOptimizer, solver) { +TEST(LinearOptimizer, solverCheckIndividually) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues expected; @@ -53,7 +53,7 @@ TEST(LinearOptimizer, solver) { expected.insert(1, Vector2(-0.1, 0.1)); LinearSolverParams params; - params.ordering = Ordering::Colamd(gfg); + params.orderingType = Ordering::COLAMD; // Below we solve with different backend linear solver choices // Note: these tests are not in a for loop to enable easier debugging @@ -82,11 +82,15 @@ TEST(LinearOptimizer, solver) { EXPECT(assert_equal(expected, actual)); // Iterative - either PCGSolver or SubgraphSolver - params.linearSolverType = LinearSolverParams::Iterative; - params.iterativeParams = boost::make_shared( - boost::make_shared()); + // first PCGSolver + params = LinearSolverParams( + LinearSolverParams::Iterative, Ordering::COLAMD, + boost::make_shared( + boost::make_shared())); actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); + // then SubgraphSolver + params.ordering = Ordering::Colamd(gfg); params.iterativeParams = boost::make_shared(); actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); @@ -98,14 +102,16 @@ TEST(LinearOptimizer, solver) { // EXPECT(assert_equal(expected, actual)); // PCG - Preconditioned Conjugate Gradient, an iterative method - params.linearSolverType = LinearSolverParams::PCG; - params.iterativeParams = boost::make_shared( - boost::make_shared()); + params = LinearSolverParams( + LinearSolverParams::PCG, Ordering::COLAMD, + boost::make_shared( + boost::make_shared())); actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Subgraph - SPCG, see SubgraphSolver.h params.linearSolverType = LinearSolverParams::SUBGRAPH; + params.ordering = Ordering::Colamd(gfg); params.iterativeParams = boost::make_shared(); actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); @@ -135,6 +141,48 @@ TEST(LinearOptimizer, solver) { #endif } +// creates a dummy iterativeParams object for the appropriate solver type +IterativeOptimizationParameters::shared_ptr createIterativeParams(int solver) { + typedef LinearSolverParams LSP; + return (solver == LSP::Iterative) || (solver == LSP::PCG) + ? boost::make_shared( + boost::make_shared()) + : (solver == LSP::SUBGRAPH) + ? boost::make_shared() + : boost::make_shared(); +} + +/* ************************************************************************* */ +TEST(LinearOptimizer, solverCheckWithLoop) { + // the same as the previous test except in a loop for consistency + GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); + + VectorValues expected; + expected.insert(2, Vector2(-0.1, -0.1)); + expected.insert(0, Vector2(0.1, -0.2)); + expected.insert(1, Vector2(-0.1, 0.1)); + + LinearSolverParams params; + params.ordering = Ordering::Colamd(gfg); + + // Test all linear solvers + typedef LinearSolverParams LSP; + for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { + if (solver == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option +#ifndef GTSAM_USE_SUITESPARSE + if (solver == LSP::SUITESPARSE_CHOLESKY) continue; +#endif +#ifndef GTSAM_USE_CUSPARSE + if (solver == LSP::CUSPARSE_CHOLESKY) continue; +#endif + auto params = LinearSolverParams(static_cast(solver), + Ordering::Colamd(gfg), + createIterativeParams(solver)); + auto actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + EXPECT(assert_equal(expected, actual)); + } +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 44f09064d9..8db7ad9d37 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -179,8 +179,7 @@ TEST(NonlinearOptimizer, optimization_method) { // Test all linear solvers typedef LinearSolverParams LSP; - for (int solver = LSP::MULTIFRONTAL_CHOLESKY; - solver != LSP::LAST; solver++) { + for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { if (solver == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option #ifndef GTSAM_USE_SUITESPARSE if (solver == LSP::SUITESPARSE_CHOLESKY) continue; From e5aa981c90abaa5d0cd76f7546344399147c95b3 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 08:58:37 -0500 Subject: [PATCH 72/91] linearSolverType variable - backwards compatibility --- gtsam/linear/CuSparseSolver.cpp | 2 ++ gtsam/linear/EliminationSolver.h | 4 +++- gtsam/linear/PCGSolver.cpp | 2 ++ gtsam/linear/SparseEigenSolver.cpp | 3 +++ gtsam/linear/SubgraphSolver.h | 5 ++++- gtsam/linear/SuiteSparseSolver.cpp | 7 ++++--- gtsam/linear/tests/testLinearSolver.cpp | 21 ++++++++++++++------- 7 files changed, 32 insertions(+), 12 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index feb9642788..a7110c200b 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -19,6 +19,7 @@ */ #include "gtsam/linear/CuSparseSolver.h" +#include "gtsam/linear/LinearSolverParams.h" #ifdef GTSAM_USE_CUSPARSE #include @@ -35,6 +36,7 @@ namespace gtsam { const Ordering &ordering) { solverType = type; this->ordering = ordering; + linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; } bool CuSparseSolver::isIterative() { diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h index 8b0134d6e7..a9239b6617 100644 --- a/gtsam/linear/EliminationSolver.h +++ b/gtsam/linear/EliminationSolver.h @@ -40,7 +40,9 @@ namespace gtsam { class GTSAM_EXPORT EliminationSolver : public LinearSolver { public: explicit EliminationSolver(const LinearSolverParams ¶ms) - : params_(params){}; + : params_(params) { + linearSolverType = params.linearSolverType; + }; bool isIterative() override { return false; }; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 08307c5abb..ade152e82a 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include #include @@ -43,6 +44,7 @@ void PCGSolverParameters::print(ostream &os) const { PCGSolver::PCGSolver(const PCGSolverParameters &p) { parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); + linearSolverType = LinearSolverParams::PCG; } /*****************************************************************************/ diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index f0dfd83f31..1afba0d828 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -227,5 +228,7 @@ namespace gtsam { SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) { solverType = type; this->ordering = ordering; + linearSolverType = (type == QR) ? LinearSolverParams::EIGEN_QR + : LinearSolverParams::EIGEN_CHOLESKY; } } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 129d5f4aae..433f51bc7a 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include // pair @@ -167,7 +168,9 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { public: SubgraphSolverWrapper(const SubgraphSolverParameters ¶meters, const Ordering &ordering) - : parameters_(parameters), ordering_(ordering) {}; + : parameters_(parameters), ordering_(ordering) { + linearSolverType = LinearSolverParams::SUBGRAPH; + }; /// satisfies LinearSolver interface to solve the GaussianFactorGraph. VectorValues solve(const GaussianFactorGraph &gfg) override { diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index c40c1fca9c..45eddcd742 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -18,9 +18,9 @@ * @author Fan Jiang */ -#include "gtsam/linear/SuiteSparseSolver.h" - -#include "gtsam/linear/SparseEigenSolver.h" +#include +#include +#include #ifdef GTSAM_USE_SUITESPARSE #include @@ -31,6 +31,7 @@ namespace gtsam { const Ordering &ordering) { solverType = type; this->ordering = ordering; + linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; } bool SuiteSparseSolver::isIterative() { diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 736a4f3973..c1fa6f8b76 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -167,19 +167,26 @@ TEST(LinearOptimizer, solverCheckWithLoop) { // Test all linear solvers typedef LinearSolverParams LSP; - for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { - if (solver == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option + for (int solverType = LSP::MULTIFRONTAL_CHOLESKY; solverType != LSP::LAST; + solverType++) { + if (solverType == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option #ifndef GTSAM_USE_SUITESPARSE - if (solver == LSP::SUITESPARSE_CHOLESKY) continue; + if (solverType == LSP::SUITESPARSE_CHOLESKY) continue; #endif #ifndef GTSAM_USE_CUSPARSE - if (solver == LSP::CUSPARSE_CHOLESKY) continue; + if (solverType == LSP::CUSPARSE_CHOLESKY) continue; #endif - auto params = LinearSolverParams(static_cast(solver), + auto params = LinearSolverParams(static_cast(solverType), Ordering::Colamd(gfg), - createIterativeParams(solver)); - auto actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + createIterativeParams(solverType)); + auto linearSolver = LinearSolver::FromLinearSolverParams(params); + auto actual = (*linearSolver)(gfg); EXPECT(assert_equal(expected, actual)); + if (solverType == LSP::Iterative) { // iterative defaults to PCG + EXPECT(LSP::PCG == linearSolver->linearSolverType); + } else { + EXPECT(solverType == linearSolver->linearSolverType); + } } } From bd50301f047c1a3d38b88e3cbd70d4cf56c0e564 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Fri, 13 Nov 2020 09:09:20 -0500 Subject: [PATCH 73/91] copy-move semantics on NonlinearOptimizerParams due to references to LinearSolverParams, we have to be careful copying+moving --- gtsam/nonlinear/NonlinearOptimizerParams.h | 35 +++++++++++++++ tests/testNonlinearOptimizer.cpp | 50 ++++++++++++++++++++++ 2 files changed, 85 insertions(+) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 8fd393451c..2722e50412 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -55,6 +55,41 @@ class GTSAM_EXPORT NonlinearOptimizerParams { errorTol(0.0), verbosity(SILENT) {} + // copy constructor + NonlinearOptimizerParams(const NonlinearOptimizerParams& other) + : maxIterations(other.maxIterations), + relativeErrorTol(other.relativeErrorTol), + absoluteErrorTol(other.absoluteErrorTol), + errorTol(other.errorTol), + verbosity(other.verbosity), + linearSolverParams(other.linearSolverParams) {} + + // move constructor + NonlinearOptimizerParams(NonlinearOptimizerParams&& other) noexcept + : maxIterations(other.maxIterations), + relativeErrorTol(other.relativeErrorTol), + absoluteErrorTol(other.absoluteErrorTol), + errorTol(other.errorTol), + verbosity(other.verbosity), + linearSolverParams(std::move(other.linearSolverParams)) {} + + // copy assignment + NonlinearOptimizerParams& operator=(const NonlinearOptimizerParams& other) { + return *this = NonlinearOptimizerParams(other); + } + + // move assignment + NonlinearOptimizerParams& operator=( + NonlinearOptimizerParams&& other) noexcept { + maxIterations = other.maxIterations; + relativeErrorTol = other.relativeErrorTol; + absoluteErrorTol = other.absoluteErrorTol; + errorTol = other.errorTol; + verbosity = other.verbosity; + std::swap(linearSolverParams, other.linearSolverParams); + return *this; + } + virtual ~NonlinearOptimizerParams() { } virtual void print(const std::string& str = "") const; diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 8db7ad9d37..ab9d48ceb2 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -257,6 +257,56 @@ TEST(NonlinearOptimizer, NullFactor) { DOUBLES_EQUAL(0,fg.error(actual3),tol); } +TEST(NonlinearOptimizerParams, RuleOfFive) { + // test copy and move constructors. + NonlinearOptimizerParams params; + typedef LinearSolverParams LSP; + params.maxIterations = 2; + params.linearSolverType = LSP::MULTIFRONTAL_QR; + + // test copy's + auto params2 = params; // copy-assignment + auto params3{params}; // copy-constructor + EXPECT(params2.maxIterations == params.maxIterations); + EXPECT(params2.linearSolverType == params.linearSolverType); + EXPECT(params.linearSolverType == + params.linearSolverParams.linearSolverType); + EXPECT(params2.linearSolverType == + params2.linearSolverParams.linearSolverType); + EXPECT(params3.maxIterations == params.maxIterations); + EXPECT(params3.linearSolverType == params.linearSolverType); + EXPECT(params3.linearSolverType == + params3.linearSolverParams.linearSolverType); + params2.linearSolverType = LSP::MULTIFRONTAL_CHOLESKY; + params3.linearSolverType = LSP::SEQUENTIAL_QR; + EXPECT(params.linearSolverType == LSP::MULTIFRONTAL_QR); + EXPECT(params.linearSolverParams.linearSolverType == LSP::MULTIFRONTAL_QR); + EXPECT(params2.linearSolverType == LSP::MULTIFRONTAL_CHOLESKY); + EXPECT(params2.linearSolverParams.linearSolverType == LSP::MULTIFRONTAL_CHOLESKY); + EXPECT(params3.linearSolverType == LSP::SEQUENTIAL_QR); + EXPECT(params3.linearSolverParams.linearSolverType == LSP::SEQUENTIAL_QR); + + // test move's + NonlinearOptimizerParams params4 = std::move(params2); // move-constructor + NonlinearOptimizerParams params5; + params5 = std::move(params3); // move-assignment + EXPECT(params4.linearSolverType == LSP::MULTIFRONTAL_CHOLESKY); + EXPECT(params4.linearSolverParams.linearSolverType == LSP::MULTIFRONTAL_CHOLESKY); + EXPECT(params5.linearSolverType == LSP::SEQUENTIAL_QR); + EXPECT(params5.linearSolverParams.linearSolverType == LSP::SEQUENTIAL_QR); + params4.linearSolverType = LSP::SEQUENTIAL_CHOLESKY; + params5.linearSolverType = LSP::EIGEN_QR; + EXPECT(params4.linearSolverType == LSP::SEQUENTIAL_CHOLESKY); + EXPECT(params4.linearSolverParams.linearSolverType == LSP::SEQUENTIAL_CHOLESKY); + EXPECT(params5.linearSolverType == LSP::EIGEN_QR); + EXPECT(params5.linearSolverParams.linearSolverType == LSP::EIGEN_QR); + + // test destructor + { + NonlinearOptimizerParams params6; + } +} + /* ************************************************************************* */ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) { From 99ef89934d736d1075d7a18a1bcf25cb8a15d705 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 14 Nov 2020 04:36:30 -0500 Subject: [PATCH 74/91] readme information for SuiteSparse and cuSPARSE --- INSTALL.md | 2 ++ README.md | 2 ++ 2 files changed, 4 insertions(+) diff --git a/INSTALL.md b/INSTALL.md index 3dbc3a850c..e8ed5b5b20 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -27,6 +27,8 @@ $ make install `GTSAM_WITH_EIGEN_MKL_OPENMP` to `ON`; however, best performance is usually achieved with MKL disabled. We therefore advise you to benchmark your problem before using MKL. + - SuiteSparse is an alternate sparse matrix solver, but it is rarely faster than GTSAM's default variable elimination implementation. Once enabled by toggling `GTSAM_WITH_SUITESPARSE`, it can be specified as the solver with `LinearSolverParams::linearSolverType`. + - cuSPARSE is the CUDA (GPU-accelerated) sparse matrix solver but is also rarely faster than GTSAM's default variable elimination implementation. Once enabled by toggling `GTSAM_WITH_CUSPARSE`, it can be specified as the solver with `LinearSolverParams::linearSolverType`. Tested compilers: diff --git a/README.md b/README.md index 60eff197ae..51aec28e68 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,8 @@ Optional prerequisites - used automatically if findable by CMake: - [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl) (Ubuntu: [installing using APT](https://software.intel.com/en-us/articles/installing-intel-free-libs-and-python-apt-repo)) - See [INSTALL.md](INSTALL.md) for more installation information - Note that MKL may not provide a speedup in all cases. Make sure to benchmark your problem with and without MKL. +- [SuiteSparse](https://people.engr.tamu.edu/davis/suitesparse.html) (Ubuntu: `sudo apt-get install libsuitesparse-dev`) +- [cuSPARSE](https://docs.nvidia.com/cuda/cusparse/index.html) (comes with [CUDA Toolkit](https://developer.nvidia.com/cuda-toolkit)) ## GTSAM 4 Compatibility From 4f897b2aa22bae9e704f6db11174df6eb6eedfee Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 15 Nov 2020 11:10:16 -0500 Subject: [PATCH 75/91] comments about METIS/SuiteSparse in CMakeLists files --- gtsam/3rdparty/CMakeLists.txt | 2 +- gtsam/CMakeLists.txt | 2 +- gtsam_unstable/partition/tests/CMakeLists.txt | 1 + 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/3rdparty/CMakeLists.txt b/gtsam/3rdparty/CMakeLists.txt index 76e6c9fad4..90eef8a009 100644 --- a/gtsam/3rdparty/CMakeLists.txt +++ b/gtsam/3rdparty/CMakeLists.txt @@ -50,7 +50,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN) endif() option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF) -if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) +if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) # SuiteSparse comes with its own version of METIS add_subdirectory(metis) endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index f1a5d0dad1..0608c0d9fc 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -87,7 +87,7 @@ configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) -if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) +if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) # SuiteSparse comes with its own version of METIS list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam) endif() diff --git a/gtsam_unstable/partition/tests/CMakeLists.txt b/gtsam_unstable/partition/tests/CMakeLists.txt index 3184ed148f..1e7a15d31e 100644 --- a/gtsam_unstable/partition/tests/CMakeLists.txt +++ b/gtsam_unstable/partition/tests/CMakeLists.txt @@ -1,4 +1,5 @@ set(ignore_test "testNestedDissection.cpp") +# SuiteSparse comes with its own version of METIS - use that instead to avoid conflicts if (NOT GTSAM_USE_SUITESPARSE) gtsamAddTestsGlob(partition "test*.cpp" "${ignore_test}" "gtsam_unstable;gtsam;metis-gtsam") else() From f4a403ef8c3e4e81e32e5a5037b117f77e2d6fe5 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 15 Nov 2020 11:45:38 -0500 Subject: [PATCH 76/91] const methods, variable _ convention, and minor formatting --- gtsam/linear/CuSparseSolver.cpp | 25 ++++++------- gtsam/linear/CuSparseSolver.h | 43 +++++++++++----------- gtsam/linear/EliminationSolver.h | 14 ++++---- gtsam/linear/GaussianFactorGraph.h | 1 - gtsam/linear/IterativeSolver.cpp | 4 +-- gtsam/linear/IterativeSolver.h | 12 +++---- gtsam/linear/LinearSolver.h | 12 ++++--- gtsam/linear/PCGSolver.cpp | 4 +-- gtsam/linear/PCGSolver.h | 2 +- gtsam/linear/SparseEigenSolver.cpp | 26 +++++++------- gtsam/linear/SparseEigenSolver.h | 47 ++++++++++++------------- gtsam/linear/SubgraphSolver.cpp | 2 +- gtsam/linear/SubgraphSolver.h | 6 ++-- gtsam/linear/SuiteSparseSolver.cpp | 27 +++++++------- gtsam/linear/SuiteSparseSolver.h | 43 +++++++++++----------- gtsam/linear/tests/testLinearSolver.cpp | 4 +-- 16 files changed, 136 insertions(+), 136 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index a7110c200b..91815765cd 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -33,17 +33,16 @@ namespace gtsam { CuSparseSolver::CuSparseSolver(CuSparseSolver::CuSparseSolverType type, - const Ordering &ordering) { - solverType = type; - this->ordering = ordering; - linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; + const Ordering &ordering) + : solverType_(type), ordering_(ordering) { + linearSolverType_ = LinearSolverParams::CUSPARSE_CHOLESKY; } - bool CuSparseSolver::isIterative() { + bool CuSparseSolver::isIterative() const { return false; } - bool CuSparseSolver::isSequential() { + bool CuSparseSolver::isSequential() const { return false; } @@ -111,14 +110,15 @@ namespace gtsam { cudaMemcpyHostToDevice)); } - VectorValues CuSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { - if (solverType == QR) { + VectorValues CuSparseSolver::solve( + const gtsam::GaussianFactorGraph &gfg) const { + if (solverType_ == QR) { throw std::invalid_argument("This solver does not support QR."); - } else if (solverType == CHOLESKY) { + } else if (solverType_ == CHOLESKY) { gttic_(CuSparseSolver_optimizeEigenCholesky); Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); auto rows = Ab.rows(), cols = Ab.cols(); Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); // @@ -247,7 +247,7 @@ namespace gtsam { { size_t currentColIndex = 0; - for (const auto key : ordering) { + for (const auto key : ordering_) { columnIndices[key] = currentColIndex; currentColIndex += dims[key]; } @@ -263,7 +263,8 @@ namespace gtsam { throw std::exception(); } #else - VectorValues CuSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + VectorValues CuSparseSolver::solve( + const gtsam::GaussianFactorGraph &gfg) const { throw std::invalid_argument("This GTSAM is compiled without CUDA support"); } #endif diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index 58dc60fdc2..bc47d3f460 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -21,36 +21,35 @@ #pragma once #include -#include #include +#include + #include namespace gtsam { - /** - * cuSparse based Backend class - */ - class GTSAM_EXPORT CuSparseSolver : public LinearSolver { - public: - - typedef enum { - QR, - CHOLESKY - } CuSparseSolverType; - - - explicit CuSparseSolver(CuSparseSolver::CuSparseSolverType type, const Ordering &ordering); - - bool isIterative() override; +/** + * cuSparse based Backend class + */ +class GTSAM_EXPORT CuSparseSolver : public LinearSolver { + public: + typedef enum { + QR, + CHOLESKY + } CuSparseSolverType; - bool isSequential() override; + protected: + CuSparseSolverType solverType_ = QR; + Ordering ordering_; - VectorValues solve(const GaussianFactorGraph &gfg) override; + public: + explicit CuSparseSolver(CuSparseSolver::CuSparseSolverType type, + const Ordering &ordering); - protected: + bool isIterative() const override; - CuSparseSolverType solverType = QR; + bool isSequential() const override; - Ordering ordering; - }; + VectorValues solve(const GaussianFactorGraph &gfg) const override; +}; } // namespace gtsam diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h index a9239b6617..05a23a3ae3 100644 --- a/gtsam/linear/EliminationSolver.h +++ b/gtsam/linear/EliminationSolver.h @@ -38,15 +38,18 @@ namespace gtsam { * "LinearSolver" unified API. */ class GTSAM_EXPORT EliminationSolver : public LinearSolver { + protected: + LinearSolverParams params_; + public: explicit EliminationSolver(const LinearSolverParams ¶ms) : params_(params) { - linearSolverType = params.linearSolverType; + linearSolverType_ = params.linearSolverType; }; - bool isIterative() override { return false; }; + bool isIterative() const override { return false; }; - bool isSequential() override { + bool isSequential() const override { return params_.linearSolverType == LinearSolverParams::SEQUENTIAL_QR || params_.linearSolverType == LinearSolverParams::SEQUENTIAL_CHOLESKY; }; @@ -56,7 +59,7 @@ class GTSAM_EXPORT EliminationSolver : public LinearSolver { * @param gfg the factor graph to solve * @returns the solution */ - VectorValues solve(const GaussianFactorGraph &gfg) override { + VectorValues solve(const GaussianFactorGraph &gfg) const override { switch (params_.linearSolverType) { case LinearSolverParams::MULTIFRONTAL_QR: return params_.ordering ? gfg.optimize(*params_.ordering, EliminateQR) @@ -88,9 +91,6 @@ class GTSAM_EXPORT EliminationSolver : public LinearSolver { "EliminationSolver"); } }; - - protected: - LinearSolverParams params_; }; } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 5c48f4c58d..78a957cc2f 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -380,7 +380,6 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; - /// @} private: diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 17a7147c7e..a1cc339116 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -90,14 +90,14 @@ string IterativeOptimizationParameters::verbosityTranslator( /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, boost::optional keyInfo, - boost::optional&> lambda) { + boost::optional&> lambda) const { return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg), lambda ? *lambda : std::map()); } /*****************************************************************************/ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda) { + const KeyInfo &keyInfo, const std::map &lambda) const { return optimize(gfg, keyInfo, lambda, keyInfo.x0()); } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 2f427577db..3caf9c52e8 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -96,25 +96,25 @@ class IterativeSolver : public LinearSolver { /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, - boost::optional&> lambda = boost::none); + boost::optional&> lambda = boost::none) const; /* interface to the nonlinear optimizer, without initial estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, - const std::map &lambda); + const std::map &lambda) const; /* interface to the nonlinear optimizer that the subclasses have to implement */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) = 0; + const VectorValues &initial) const = 0; /* satisfies LinearSolver interface */ - bool isIterative() override { return true; }; + bool isIterative() const override { return true; }; /* satisfies LinearSolver interface */ - bool isSequential() override { return false; }; + bool isSequential() const override { return false; }; /* satisfies LinearSolver interface */ - VectorValues solve(const GaussianFactorGraph &gfg) override { + VectorValues solve(const GaussianFactorGraph &gfg) const override { return optimize(gfg); }; diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 7d90680095..b12e0a4ca5 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -28,19 +28,19 @@ class GTSAM_EXPORT LinearSolver { LinearSolver(LinearSolver &) = delete; // TODO: Remove this and use trait functions instead? - LinearSolverParams::LinearSolverType linearSolverType = + LinearSolverParams::LinearSolverType linearSolverType_ = LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of this instance - virtual bool isIterative() { return false; }; + virtual bool isIterative() const { return false; }; - virtual bool isSequential() { return false; }; + virtual bool isSequential() const { return false; }; /** * Solve a Gaussian Factor Graph with the solver * @param gfg the GFG to be optimized * @return the optimization result in VectorValues */ - virtual VectorValues solve(const GaussianFactorGraph &gfg) { + virtual VectorValues solve(const GaussianFactorGraph &gfg) const { throw std::runtime_error("BUG_CHECK: Calling solve of the base class!"); }; @@ -49,7 +49,9 @@ class GTSAM_EXPORT LinearSolver { * @param gfg the GFG to be optimized * @return the optimization result in VectorValues */ - VectorValues operator()(const GaussianFactorGraph &gfg) { return solve(gfg); } + VectorValues operator()(const GaussianFactorGraph &gfg) const { + return solve(gfg); + } /** * Factor method for generating a LinearSolver from LinearSolverParams diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index f06fc8a196..590672d6f4 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -44,7 +44,7 @@ void PCGSolverParameters::print(ostream &os) const { PCGSolver::PCGSolver(const PCGSolverParameters &p) { parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); - linearSolverType = LinearSolverParams::PCG; + linearSolverType_ = LinearSolverParams::PCG; } void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr preconditioner) { @@ -61,7 +61,7 @@ void PCGSolverParameters::print(const std::string &s) const { /*****************************************************************************/ VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) { + const VectorValues &initial) const { /* build preconditioner */ preconditioner_->build(gfg, keyInfo, lambda); diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index ef97866b50..f9eb5cc6ad 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -81,7 +81,7 @@ class GTSAM_EXPORT PCGSolver: public IterativeSolver { VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) override; + const VectorValues &initial) const override; }; diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 1afba0d828..92fb3ffbf3 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -146,18 +146,18 @@ namespace gtsam { Ab.col(cols)); } - bool SparseEigenSolver::isIterative() { + bool SparseEigenSolver::isIterative() const { return false; } - bool SparseEigenSolver::isSequential() { + bool SparseEigenSolver::isSequential() const { return false; } - VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) { - if (solverType == QR) { + VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) const { + if (solverType_ == QR) { gttic_(EigenOptimizer_optimizeEigenQR); - auto Ab_pair = obtainSparseMatrix(gfg, ordering); + auto Ab_pair = obtainSparseMatrix(gfg, ordering_); // Solve A*x = b using sparse QR from Eigen gttic_(EigenOptimizer_optimizeEigenQR_create_solver); @@ -169,9 +169,9 @@ namespace gtsam { gttoc_(EigenOptimizer_optimizeEigenQR_solve); return VectorValues(x, gfg.getKeyDimMap()); - } else if (solverType == CHOLESKY) { + } else if (solverType_ == CHOLESKY) { gttic_(EigenOptimizer_optimizeEigenCholesky); - SpMat Ab = sparseJacobianEigen(gfg, ordering); + SpMat Ab = sparseJacobianEigen(gfg, ordering_); auto rows = Ab.rows(), cols = Ab.cols(); auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); @@ -209,7 +209,7 @@ namespace gtsam { { size_t currentColIndex = 0; - for (const auto key : ordering) { + for (const auto key : ordering_) { columnIndices[key] = currentColIndex; currentColIndex += dims[key]; } @@ -225,10 +225,10 @@ namespace gtsam { throw std::exception(); } - SparseEigenSolver::SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) { - solverType = type; - this->ordering = ordering; - linearSolverType = (type == QR) ? LinearSolverParams::EIGEN_QR - : LinearSolverParams::EIGEN_CHOLESKY; + SparseEigenSolver::SparseEigenSolver( + SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) + : solverType_(type), ordering_(ordering) { + linearSolverType_ = (type == QR) ? LinearSolverParams::EIGEN_QR + : LinearSolverParams::EIGEN_CHOLESKY; } } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index d71365864e..0d798b682b 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -23,40 +23,39 @@ #pragma once #include -#include #include +#include + #include #include namespace gtsam { - /** - * Eigen SparseSolver based Backend class - */ - class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { - public: - - typedef enum { - QR, - CHOLESKY - } SparseEigenSolverType; - - - explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering); - - bool isIterative() override; +/** + * Eigen SparseSolver based Backend class + */ +class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { + public: + typedef enum { + QR, + CHOLESKY + } SparseEigenSolverType; - bool isSequential() override; + protected: + SparseEigenSolverType solverType_ = QR; + Ordering ordering_; - VectorValues solve(const GaussianFactorGraph &gfg) override; + public: + explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, + const Ordering &ordering); - static Eigen::SparseMatrix - sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering); + bool isIterative() const override; - protected: + bool isSequential() const override; - SparseEigenSolverType solverType = QR; + VectorValues solve(const GaussianFactorGraph &gfg) const override; - Ordering ordering; - }; + static Eigen::SparseMatrix sparseJacobianEigen( + const GaussianFactorGraph &gfg, const Ordering &ordering); +}; } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index f49f9a135f..90b896aedb 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -74,7 +74,7 @@ VectorValues SubgraphSolver::optimize() const { VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const map &lambda, - const VectorValues &initial) { + const VectorValues &initial) const { return VectorValues(); } /**************************************************************************************************/ diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index d03937fa61..81d6cd102e 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -127,7 +127,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) override; + const VectorValues &initial) const override; /// @} /// @name Implement interface @@ -152,11 +152,11 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { SubgraphSolverWrapper(const SubgraphSolverParameters ¶meters, const Ordering &ordering) : parameters_(parameters), ordering_(ordering) { - linearSolverType = LinearSolverParams::SUBGRAPH; + linearSolverType_ = LinearSolverParams::SUBGRAPH; }; /// satisfies LinearSolver interface to solve the GaussianFactorGraph. - VectorValues solve(const GaussianFactorGraph &gfg) override { + VectorValues solve(const GaussianFactorGraph &gfg) const override { return SubgraphSolver(gfg, parameters_, ordering_).optimize(); }; diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index 45eddcd742..f34e229923 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -27,29 +27,29 @@ #endif namespace gtsam { - SuiteSparseSolver::SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, - const Ordering &ordering) { - solverType = type; - this->ordering = ordering; - linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; + SuiteSparseSolver::SuiteSparseSolver( + SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering) + : solverType_(type), ordering_(ordering) { + linearSolverType_ = LinearSolverParams::SUITESPARSE_CHOLESKY; } - bool SuiteSparseSolver::isIterative() { + bool SuiteSparseSolver::isIterative() const { return false; } - bool SuiteSparseSolver::isSequential() { + bool SuiteSparseSolver::isSequential() const { return false; } #ifdef GTSAM_USE_SUITESPARSE - VectorValues SuiteSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { - if (solverType == QR) { + VectorValues SuiteSparseSolver::solve( + const gtsam::GaussianFactorGraph &gfg) const { + if (solverType_ == QR) { throw std::invalid_argument("This solver does not support QR."); - } else if (solverType == CHOLESKY) { + } else if (solverType_ == CHOLESKY) { gttic_(SuiteSparseSolver_optimizeEigenCholesky); Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); auto rows = Ab.rows(), cols = Ab.cols(); auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); @@ -94,7 +94,7 @@ namespace gtsam { { size_t currentColIndex = 0; - for (const auto key : ordering) { + for (const auto key : ordering_) { columnIndices[key] = currentColIndex; currentColIndex += dims[key]; } @@ -110,7 +110,8 @@ namespace gtsam { throw std::exception(); } #else - VectorValues SuiteSparseSolver::solve(const gtsam::GaussianFactorGraph &gfg) { + VectorValues SuiteSparseSolver::solve( + const gtsam::GaussianFactorGraph &gfg) const { throw std::invalid_argument( "This GTSAM is compiled without SuiteSparse support"); } diff --git a/gtsam/linear/SuiteSparseSolver.h b/gtsam/linear/SuiteSparseSolver.h index 67310f390f..ea13719906 100644 --- a/gtsam/linear/SuiteSparseSolver.h +++ b/gtsam/linear/SuiteSparseSolver.h @@ -21,36 +21,35 @@ #pragma once #include -#include #include +#include + #include namespace gtsam { - /** - * Eigen SparseSolver based Backend class - */ - class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { - public: - - typedef enum { - QR, - CHOLESKY - } SuiteSparseSolverType; - - - explicit SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering); - - bool isIterative() override; +/** + * Eigen SparseSolver based Backend class + */ +class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { + public: + typedef enum { + QR, + CHOLESKY + } SuiteSparseSolverType; - bool isSequential() override; + protected: + SuiteSparseSolverType solverType_ = QR; + Ordering ordering_; - VectorValues solve(const GaussianFactorGraph &gfg) override; + public: + explicit SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, + const Ordering &ordering); - protected: + bool isIterative() const override; - SuiteSparseSolverType solverType = QR; + bool isSequential() const override; - Ordering ordering; - }; + VectorValues solve(const GaussianFactorGraph &gfg) const override; +}; } // namespace gtsam diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index c1fa6f8b76..5c67d339b0 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -183,9 +183,9 @@ TEST(LinearOptimizer, solverCheckWithLoop) { auto actual = (*linearSolver)(gfg); EXPECT(assert_equal(expected, actual)); if (solverType == LSP::Iterative) { // iterative defaults to PCG - EXPECT(LSP::PCG == linearSolver->linearSolverType); + EXPECT(LSP::PCG == linearSolver->linearSolverType_); } else { - EXPECT(solverType == linearSolver->linearSolverType); + EXPECT(solverType == linearSolver->linearSolverType_); } } } From 05d52f26215c835fab3231e81aa9295211a6f4a2 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 15 Nov 2020 12:02:35 -0500 Subject: [PATCH 77/91] comments about variable ordering for sparse matrix generation --- gtsam/linear/CuSparseSolver.cpp | 1 + gtsam/linear/CuSparseSolver.h | 9 ++++++++- gtsam/linear/SparseEigenSolver.cpp | 4 +++- gtsam/linear/SparseEigenSolver.h | 9 +++++++++ gtsam/linear/SuiteSparseSolver.cpp | 2 ++ gtsam/linear/SuiteSparseSolver.h | 7 +++++++ 6 files changed, 30 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index 91815765cd..3f7b0fd255 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -117,6 +117,7 @@ namespace gtsam { } else if (solverType_ == CHOLESKY) { gttic_(CuSparseSolver_optimizeEigenCholesky); + // ordering is used here Eigen::SparseMatrix Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); auto rows = Ab.rows(), cols = Ab.cols(); diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index bc47d3f460..5ed2fd4507 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -12,7 +12,10 @@ /** * @file CuSparseSolver.h * - * @brief CuSparse based linear solver backend for GTSAM + * @brief CuSparse based linear solver backend for GTSAM. + * + * Generates a sparse matrix with given ordering and calls the cuSPARSE solver + * to solve it. * * @date Jun 2020 * @author Fan Jiang @@ -50,6 +53,10 @@ class GTSAM_EXPORT CuSparseSolver : public LinearSolver { bool isSequential() const override; + /** Solves the GaussianFactorGraph using a sparse matrix solver + * + * Uses elimination ordering during sparse matrix generation + */ VectorValues solve(const GaussianFactorGraph &gfg) const override; }; } // namespace gtsam diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 92fb3ffbf3..0119cf7dbf 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -112,7 +112,7 @@ namespace gtsam { } - /// obtain sparse matrix for eigen sparse solver + /// obtain sparse matrix with given variable ordering for eigen sparse solver std::pair obtainSparseMatrix( const GaussianFactorGraph &gfg, const Ordering &ordering) { @@ -157,6 +157,8 @@ namespace gtsam { VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) const { if (solverType_ == QR) { gttic_(EigenOptimizer_optimizeEigenQR); + + // this is where ordering is used auto Ab_pair = obtainSparseMatrix(gfg, ordering_); // Solve A*x = b using sparse QR from Eigen diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 0d798b682b..56cd6d7e4a 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -14,6 +14,9 @@ * * @brief Eigen SparseSolver based linear solver backend for GTSAM * + * Generates a sparse matrix with given ordering and calls the Eigen sparse + * matrix solver to solve it. + * * @date Aug 2019 * @author Mandy Xie * @author Fan Jiang @@ -53,8 +56,14 @@ class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { bool isSequential() const override; + /** Solves the GaussianFactorGraph using a sparse matrix solver + * + * Uses elimination ordering during sparse matrix generation in `solve(gfg)` + */ VectorValues solve(const GaussianFactorGraph &gfg) const override; + /// Returns sparse matrix with given variable ordering for sparse matrix + /// solvers static Eigen::SparseMatrix sparseJacobianEigen( const GaussianFactorGraph &gfg, const Ordering &ordering); }; diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index f34e229923..a9f09bc02a 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -48,6 +48,8 @@ namespace gtsam { throw std::invalid_argument("This solver does not support QR."); } else if (solverType_ == CHOLESKY) { gttic_(SuiteSparseSolver_optimizeEigenCholesky); + + // this is where ordering is used Eigen::SparseMatrix Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); auto rows = Ab.rows(), cols = Ab.cols(); diff --git a/gtsam/linear/SuiteSparseSolver.h b/gtsam/linear/SuiteSparseSolver.h index ea13719906..f972fa48cd 100644 --- a/gtsam/linear/SuiteSparseSolver.h +++ b/gtsam/linear/SuiteSparseSolver.h @@ -14,6 +14,9 @@ * * @brief SuiteSparse based linear solver backend for GTSAM * + * Generates a sparse matrix with given ordering and calls the SuiteSparse + * solver to solve it. + * * @date Jun 2020 * @author Fan Jiang */ @@ -50,6 +53,10 @@ class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { bool isSequential() const override; + /** Solves the GaussianFactorGraph using a sparse matrix solver + * + * Uses elimination ordering during sparse matrix generation + */ VectorValues solve(const GaussianFactorGraph &gfg) const override; }; } // namespace gtsam From 82b579f50296f4f6f87f71a0f89afaec49f49508 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 22 Nov 2020 21:57:23 -0500 Subject: [PATCH 78/91] remove unused "LinearSolver::linearSolverType_" --- gtsam/linear/CuSparseSolver.cpp | 4 +--- gtsam/linear/EliminationSolver.h | 4 +--- gtsam/linear/LinearSolver.h | 4 ---- gtsam/linear/PCGSolver.cpp | 8 +++----- gtsam/linear/SparseEigenSolver.cpp | 5 +---- gtsam/linear/SubgraphSolver.h | 4 +--- gtsam/linear/SuiteSparseSolver.cpp | 4 +--- gtsam/linear/tests/testLinearSolver.cpp | 5 ----- 8 files changed, 8 insertions(+), 30 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index 3f7b0fd255..56624c3e4d 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -34,9 +34,7 @@ namespace gtsam { CuSparseSolver::CuSparseSolver(CuSparseSolver::CuSparseSolverType type, const Ordering &ordering) - : solverType_(type), ordering_(ordering) { - linearSolverType_ = LinearSolverParams::CUSPARSE_CHOLESKY; - } + : solverType_(type), ordering_(ordering) {} bool CuSparseSolver::isIterative() const { return false; diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h index 05a23a3ae3..da95d93179 100644 --- a/gtsam/linear/EliminationSolver.h +++ b/gtsam/linear/EliminationSolver.h @@ -43,9 +43,7 @@ class GTSAM_EXPORT EliminationSolver : public LinearSolver { public: explicit EliminationSolver(const LinearSolverParams ¶ms) - : params_(params) { - linearSolverType_ = params.linearSolverType; - }; + : params_(params) {}; bool isIterative() const override { return false; }; diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index b12e0a4ca5..ae0647edf3 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -27,10 +27,6 @@ class GTSAM_EXPORT LinearSolver { public: LinearSolver(LinearSolver &) = delete; - // TODO: Remove this and use trait functions instead? - LinearSolverParams::LinearSolverType linearSolverType_ = - LinearSolverParams::MULTIFRONTAL_CHOLESKY; ///< The type of this instance - virtual bool isIterative() const { return false; }; virtual bool isSequential() const { return false; }; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 590672d6f4..00dc946e38 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -41,11 +41,9 @@ void PCGSolverParameters::print(ostream &os) const { } /*****************************************************************************/ -PCGSolver::PCGSolver(const PCGSolverParameters &p) { - parameters_ = p; - preconditioner_ = createPreconditioner(p.preconditioner_); - linearSolverType_ = LinearSolverParams::PCG; -} +PCGSolver::PCGSolver(const PCGSolverParameters &p) + : parameters_(p), + preconditioner_(createPreconditioner(p.preconditioner_)) {} void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr preconditioner) { preconditioner_ = preconditioner; diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 0119cf7dbf..fee16e54ac 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -229,8 +229,5 @@ namespace gtsam { SparseEigenSolver::SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) - : solverType_(type), ordering_(ordering) { - linearSolverType_ = (type == QR) ? LinearSolverParams::EIGEN_QR - : LinearSolverParams::EIGEN_CHOLESKY; - } + : solverType_(type), ordering_(ordering) {} } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 81d6cd102e..3c1b7d6350 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -151,9 +151,7 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { public: SubgraphSolverWrapper(const SubgraphSolverParameters ¶meters, const Ordering &ordering) - : parameters_(parameters), ordering_(ordering) { - linearSolverType_ = LinearSolverParams::SUBGRAPH; - }; + : parameters_(parameters), ordering_(ordering) {}; /// satisfies LinearSolver interface to solve the GaussianFactorGraph. VectorValues solve(const GaussianFactorGraph &gfg) const override { diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index a9f09bc02a..2333d7be4c 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -29,9 +29,7 @@ namespace gtsam { SuiteSparseSolver::SuiteSparseSolver( SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering) - : solverType_(type), ordering_(ordering) { - linearSolverType_ = LinearSolverParams::SUITESPARSE_CHOLESKY; - } + : solverType_(type), ordering_(ordering) {} bool SuiteSparseSolver::isIterative() const { return false; diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 5c67d339b0..cc3932f2ff 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -182,11 +182,6 @@ TEST(LinearOptimizer, solverCheckWithLoop) { auto linearSolver = LinearSolver::FromLinearSolverParams(params); auto actual = (*linearSolver)(gfg); EXPECT(assert_equal(expected, actual)); - if (solverType == LSP::Iterative) { // iterative defaults to PCG - EXPECT(LSP::PCG == linearSolver->linearSolverType_); - } else { - EXPECT(solverType == linearSolver->linearSolverType_); - } } } From ac331276b1ea4964986289ecb1f86d6db4dcedb3 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 00:32:53 -0500 Subject: [PATCH 79/91] LinearSolver abstract class change impls. to pure virtual --- gtsam/linear/LinearSolver.cpp | 2 -- gtsam/linear/LinearSolver.h | 21 +++++++++------------ gtsam/linear/SubgraphSolver.h | 7 +++++++ 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 3a42b76efe..7200439eb8 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -26,8 +26,6 @@ namespace gtsam { -LinearSolver::LinearSolver() = default; - boost::shared_ptr LinearSolver::FromLinearSolverParams( const LinearSolverParams ¶ms) { switch (params.linearSolverType) { diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index ae0647edf3..f5a17883a5 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -24,21 +24,22 @@ namespace gtsam { /** Common Interface Class for all linear solvers */ class GTSAM_EXPORT LinearSolver { + protected: + // default constructor needed in order to delete copy constructor + LinearSolver() = default; public: LinearSolver(LinearSolver &) = delete; - virtual bool isIterative() const { return false; }; + virtual bool isIterative() const = 0; - virtual bool isSequential() const { return false; }; + virtual bool isSequential() const = 0; /** * Solve a Gaussian Factor Graph with the solver * @param gfg the GFG to be optimized * @return the optimization result in VectorValues */ - virtual VectorValues solve(const GaussianFactorGraph &gfg) const { - throw std::runtime_error("BUG_CHECK: Calling solve of the base class!"); - }; + virtual VectorValues solve(const GaussianFactorGraph &gfg) const = 0; /** * Alias for `solve` @@ -50,17 +51,13 @@ class GTSAM_EXPORT LinearSolver { } /** - * Factor method for generating a LinearSolver from LinearSolverParams + * Factor method for generating a derived class of LinearSolver from + * LinearSolverParams * @param params LinearSolverParams linear optimizer parameters - * @return pointer to a LinearSolver object + * @return pointer to a LinearSolver-derived object */ static boost::shared_ptr FromLinearSolverParams( const LinearSolverParams ¶ms); - - protected: - LinearSolver(); - - virtual ~LinearSolver() = default; }; } // namespace gtsam \ No newline at end of file diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 3c1b7d6350..20775b903e 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -146,6 +146,7 @@ class GTSAM_EXPORT SubgraphSolver : public IterativeSolver { * LinearSolver interface. Specifically, rather than partitioning the * subgraph during construction, instead the partitioning will occur during * "solve" since the GaussianFactorGraph is needed to partition the graph. + * TODO(gerry): figure out a better IterativeSolver API solution */ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { public: @@ -153,6 +154,12 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { const Ordering &ordering) : parameters_(parameters), ordering_(ordering) {}; + /* satisfies LinearSolver interface */ + bool isIterative() const override { return true; }; + + /* satisfies LinearSolver interface */ + bool isSequential() const override { return false; }; + /// satisfies LinearSolver interface to solve the GaussianFactorGraph. VectorValues solve(const GaussianFactorGraph &gfg) const override { return SubgraphSolver(gfg, parameters_, ordering_).optimize(); From 9972f9bf509125bce2def9318356e400801d13a0 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 00:33:36 -0500 Subject: [PATCH 80/91] EOF newlines --- cmake/FindCUDAToolkit.cmake | 2 +- cmake/FindSuiteSparse.cmake | 2 +- gtsam/linear/IterativeSolver.cpp | 1 - gtsam/linear/LinearSolver.h | 2 +- gtsam/linear/LinearSolverParams.cpp | 2 +- gtsam/linear/LinearSolverParams.h | 2 +- gtsam/linear/SparseEigenSolver.cpp | 2 +- tests/testPCGSolver.cpp | 1 - 8 files changed, 6 insertions(+), 8 deletions(-) diff --git a/cmake/FindCUDAToolkit.cmake b/cmake/FindCUDAToolkit.cmake index db302c8e56..f1007e405c 100644 --- a/cmake/FindCUDAToolkit.cmake +++ b/cmake/FindCUDAToolkit.cmake @@ -730,4 +730,4 @@ endif() if(_CUDAToolkit_Pop_ROOT_PATH) list(REMOVE_AT CMAKE_FIND_ROOT_PATH 0) unset(_CUDAToolkit_Pop_ROOT_PATH) -endif() \ No newline at end of file +endif() diff --git a/cmake/FindSuiteSparse.cmake b/cmake/FindSuiteSparse.cmake index e38333700c..789c902a2b 100644 --- a/cmake/FindSuiteSparse.cmake +++ b/cmake/FindSuiteSparse.cmake @@ -527,4 +527,4 @@ else (SUITESPARSE_FOUND) find_package_handle_standard_args(SuiteSparse REQUIRED_VARS ${SUITESPARSE_FOUND_REQUIRED_VARS} FAIL_MESSAGE "Failed to find some/all required components of SuiteSparse.") -endif (SUITESPARSE_FOUND) \ No newline at end of file +endif (SUITESPARSE_FOUND) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index a1cc339116..ca0a183fb7 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -176,4 +176,3 @@ Vector KeyInfo::x0vector() const { } } - diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index f5a17883a5..86e928d4fd 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -60,4 +60,4 @@ class GTSAM_EXPORT LinearSolver { const LinearSolverParams ¶ms); }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index ef55bc39ac..0aeb7a8a68 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -119,4 +119,4 @@ Ordering::OrderingType LinearSolverParams::orderingTypeTranslator( "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); } -} \ No newline at end of file +} diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 16ae92c71c..74d6f699c3 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -168,4 +168,4 @@ struct GTSAM_EXPORT LinearSolverParams { Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; }; -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index fee16e54ac..dc8e17c656 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -230,4 +230,4 @@ namespace gtsam { SparseEigenSolver::SparseEigenSolver( SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) : solverType_(type), ordering_(ordering) {} -} // namespace gtsam \ No newline at end of file +} // namespace gtsam diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index 3271cdcd20..c351846e30 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -190,4 +190,3 @@ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } - From c203774c513f53cd396647554df6a543bbf816c3 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 00:53:14 -0500 Subject: [PATCH 81/91] Remove "isIterative" and "isSequential" functions from Solver Note that Solver doesn't need these functions - only Params needs to know. --- gtsam/linear/CuSparseSolver.cpp | 8 -------- gtsam/linear/CuSparseSolver.h | 4 ---- gtsam/linear/EliminationSolver.h | 7 ------- gtsam/linear/IterativeSolver.h | 6 ------ gtsam/linear/LinearSolver.h | 4 ---- gtsam/linear/SparseEigenSolver.cpp | 8 -------- gtsam/linear/SparseEigenSolver.h | 4 ---- gtsam/linear/SubgraphSolver.h | 6 ------ gtsam/linear/SuiteSparseSolver.cpp | 8 -------- gtsam/linear/SuiteSparseSolver.h | 4 ---- 10 files changed, 59 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index 56624c3e4d..c3a20ad270 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -36,14 +36,6 @@ namespace gtsam { const Ordering &ordering) : solverType_(type), ordering_(ordering) {} - bool CuSparseSolver::isIterative() const { - return false; - } - - bool CuSparseSolver::isSequential() const { - return false; - } - #ifdef GTSAM_USE_CUSPARSE #define S1(x) #x diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index 5ed2fd4507..759a972e2c 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -49,10 +49,6 @@ class GTSAM_EXPORT CuSparseSolver : public LinearSolver { explicit CuSparseSolver(CuSparseSolver::CuSparseSolverType type, const Ordering &ordering); - bool isIterative() const override; - - bool isSequential() const override; - /** Solves the GaussianFactorGraph using a sparse matrix solver * * Uses elimination ordering during sparse matrix generation diff --git a/gtsam/linear/EliminationSolver.h b/gtsam/linear/EliminationSolver.h index da95d93179..59e46e3b52 100644 --- a/gtsam/linear/EliminationSolver.h +++ b/gtsam/linear/EliminationSolver.h @@ -45,13 +45,6 @@ class GTSAM_EXPORT EliminationSolver : public LinearSolver { explicit EliminationSolver(const LinearSolverParams ¶ms) : params_(params) {}; - bool isIterative() const override { return false; }; - - bool isSequential() const override { - return params_.linearSolverType == LinearSolverParams::SEQUENTIAL_QR || - params_.linearSolverType == LinearSolverParams::SEQUENTIAL_CHOLESKY; - }; - /** * Solve the Gaussian factor graph using variable elimination. * @param gfg the factor graph to solve diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 3caf9c52e8..624c8b9d97 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -107,12 +107,6 @@ class IterativeSolver : public LinearSolver { const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial) const = 0; - /* satisfies LinearSolver interface */ - bool isIterative() const override { return true; }; - - /* satisfies LinearSolver interface */ - bool isSequential() const override { return false; }; - /* satisfies LinearSolver interface */ VectorValues solve(const GaussianFactorGraph &gfg) const override { return optimize(gfg); diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index 86e928d4fd..db2c9094db 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -30,10 +30,6 @@ class GTSAM_EXPORT LinearSolver { public: LinearSolver(LinearSolver &) = delete; - virtual bool isIterative() const = 0; - - virtual bool isSequential() const = 0; - /** * Solve a Gaussian Factor Graph with the solver * @param gfg the GFG to be optimized diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index dc8e17c656..288f0065bc 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -146,14 +146,6 @@ namespace gtsam { Ab.col(cols)); } - bool SparseEigenSolver::isIterative() const { - return false; - } - - bool SparseEigenSolver::isSequential() const { - return false; - } - VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) const { if (solverType_ == QR) { gttic_(EigenOptimizer_optimizeEigenQR); diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 56cd6d7e4a..be305654fe 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -52,10 +52,6 @@ class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering); - bool isIterative() const override; - - bool isSequential() const override; - /** Solves the GaussianFactorGraph using a sparse matrix solver * * Uses elimination ordering during sparse matrix generation in `solve(gfg)` diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 20775b903e..eb51b4ec45 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -154,12 +154,6 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { const Ordering &ordering) : parameters_(parameters), ordering_(ordering) {}; - /* satisfies LinearSolver interface */ - bool isIterative() const override { return true; }; - - /* satisfies LinearSolver interface */ - bool isSequential() const override { return false; }; - /// satisfies LinearSolver interface to solve the GaussianFactorGraph. VectorValues solve(const GaussianFactorGraph &gfg) const override { return SubgraphSolver(gfg, parameters_, ordering_).optimize(); diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index 2333d7be4c..4a1030fdb6 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -31,14 +31,6 @@ namespace gtsam { SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering) : solverType_(type), ordering_(ordering) {} - bool SuiteSparseSolver::isIterative() const { - return false; - } - - bool SuiteSparseSolver::isSequential() const { - return false; - } - #ifdef GTSAM_USE_SUITESPARSE VectorValues SuiteSparseSolver::solve( const gtsam::GaussianFactorGraph &gfg) const { diff --git a/gtsam/linear/SuiteSparseSolver.h b/gtsam/linear/SuiteSparseSolver.h index f972fa48cd..6866ccbf66 100644 --- a/gtsam/linear/SuiteSparseSolver.h +++ b/gtsam/linear/SuiteSparseSolver.h @@ -49,10 +49,6 @@ class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { explicit SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering); - bool isIterative() const override; - - bool isSequential() const override; - /** Solves the GaussianFactorGraph using a sparse matrix solver * * Uses elimination ordering during sparse matrix generation From 29597eb8e8c43e6b4df13e19f19b7fdf089509a7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 00:55:43 -0500 Subject: [PATCH 82/91] comments and formatting --- gtsam/linear/LinearSolverParams.h | 23 +++++++++++++++++----- gtsam/nonlinear/NonlinearOptimizerParams.h | 18 ++++++++++++----- 2 files changed, 31 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 74d6f699c3..8ab8f06992 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -80,11 +80,16 @@ struct GTSAM_EXPORT LinearSolverParams { ordering(ordering), iterativeParams(iterativeParams) {}; - LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; ///< The type of linear solver to use in the nonlinear optimizer - Ordering::OrderingType orderingType = Ordering::COLAMD; ///< The method of ordering use during variable elimination (default COLAMD) - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + /// The type of linear solver to use in the nonlinear optimizer + /// (default: MULTIFRONTAL_CHOLESKY) + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; + /// The method of ordering use during variable elimination (default: COLAMD) + Ordering::OrderingType orderingType = Ordering::COLAMD; + /// The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; - boost::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + /// The container for iterativeOptimization parameters. used in CG Solvers. + boost::shared_ptr iterativeParams; inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || @@ -161,11 +166,19 @@ struct GTSAM_EXPORT LinearSolverParams { orderingType = orderingTypeTranslator(ordering); } -private: + private: + /** + * @name These functions convert between the string-name and enum versions of + * these enums. For example, + * "SEQUENTIAL_CHOLESKY" <-> LinearSolverType::SEQUENTIAL_CHOLESKY + * "METIS" <-> Ordering::METIS + * @{ + */ std::string linearSolverTranslator(LinearSolverType linearSolverType) const; LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; std::string orderingTypeTranslator(Ordering::OrderingType type) const; Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; + /**@}*/ }; } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 2722e50412..593b1d8344 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -115,8 +115,10 @@ class GTSAM_EXPORT NonlinearOptimizerParams { LinearSolverParams linearSolverParams; /** - * @name Linear Properties for Backwards Compatibility - * These member variables and functions reference LinearSolverParams + * @name Linear Properties (for Backwards Compatibility) + * These member variables and functions reference LinearSolverParams but must + * be accessible as members of NonlinearOptimizerParams for backwards + * compatibility reasons */ ///@{ @@ -136,10 +138,16 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static constexpr LinearSolverType CUSPARSE_CHOLESKY = LinearSolverParams::CUSPARSE_CHOLESKY; static constexpr LinearSolverType LAST = LinearSolverParams::LAST; - LinearSolverType &linearSolverType {linearSolverParams.linearSolverType}; ///< The type of linear solver to use in the nonlinear optimizer + /// The type of linear solver to use in the nonlinear optimizer + /// (default: MULTIFRONTAL_CHOLESKY) + LinearSolverType &linearSolverType {linearSolverParams.linearSolverType}; + /// The method of ordering use during variable elimination (default: COLAMD) Ordering::OrderingType &orderingType {linearSolverParams.orderingType}; - boost::optional &ordering {linearSolverParams.ordering}; ///< The optional variable elimination ordering, or empty to use COLAMD (default: empty) - boost::shared_ptr &iterativeParams {linearSolverParams.iterativeParams}; ///< The container for iterativeOptimization parameters. used in CG Solvers. + /// The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional &ordering {linearSolverParams.ordering}; + /// The container for iterativeOptimization parameters. used in CG Solvers. + boost::shared_ptr& iterativeParams{ + linearSolverParams.iterativeParams}; inline bool isMultifrontal() const { return linearSolverParams.isMultifrontal(); From 9560f9c17a529e36006f2af0a2174b1df262d59b Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 01:07:35 -0500 Subject: [PATCH 83/91] rename "FromLinearSolver" -> "CreateFromParameters" --- gtsam/linear/IterativeSolver.cpp | 2 +- gtsam/linear/IterativeSolver.h | 2 +- gtsam/linear/LinearSolver.cpp | 10 ++++---- gtsam/linear/LinearSolver.h | 4 +-- gtsam/linear/tests/testLinearSolver.cpp | 34 ++++++++++++------------- gtsam/nonlinear/NonlinearOptimizer.cpp | 3 ++- 6 files changed, 28 insertions(+), 27 deletions(-) diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index ca0a183fb7..4cfb18422f 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -102,7 +102,7 @@ VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg, } /*****************************************************************************/ -boost::shared_ptr IterativeSolver::FromLinearSolverParams( +boost::shared_ptr IterativeSolver::CreateFromParameters( const LinearSolverParams ¶ms) { if (!params.iterativeParams) { throw std::runtime_error( diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 624c8b9d97..5d15797559 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -113,7 +113,7 @@ class IterativeSolver : public LinearSolver { }; /* constructs PCGSolver or SubgraphSolver pointer */ - static boost::shared_ptr FromLinearSolverParams( + static boost::shared_ptr CreateFromParameters( const LinearSolverParams ¶ms); }; diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 7200439eb8..1fdd5152f9 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -26,7 +26,7 @@ namespace gtsam { -boost::shared_ptr LinearSolver::FromLinearSolverParams( +boost::shared_ptr LinearSolver::CreateFromParameters( const LinearSolverParams ¶ms) { switch (params.linearSolverType) { case LinearSolverParams::MULTIFRONTAL_QR: @@ -35,11 +35,11 @@ boost::shared_ptr LinearSolver::FromLinearSolverParams( case LinearSolverParams::SEQUENTIAL_CHOLESKY: return boost::make_shared(params); case LinearSolverParams::Iterative: - return IterativeSolver::FromLinearSolverParams(params); + return IterativeSolver::CreateFromParameters(params); case LinearSolverParams::PCG: if (!params.iterativeParams) throw std::runtime_error( - "LinearSolver::FromLinearSolverParams: iterative params has to be " + "LinearSolver::CreateFromParameters: iterative params has to be " "assigned ..."); return boost::make_shared( *boost::static_pointer_cast( @@ -47,11 +47,11 @@ boost::shared_ptr LinearSolver::FromLinearSolverParams( case LinearSolverParams::SUBGRAPH: if (!params.iterativeParams) throw std::runtime_error( - "LinearSolver::FromLinearSolverParams: iterative params has to be " + "LinearSolver::CreateFromParameters: iterative params has to be " "assigned ..."); if (!params.ordering) throw std::runtime_error( - "LinearSolver::FromLinearSolverParams: SubgraphSolver needs an " + "LinearSolver::CreateFromParameters: SubgraphSolver needs an " "ordering"); return boost::make_shared( *boost::static_pointer_cast( diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index db2c9094db..d1b6e28547 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -47,12 +47,12 @@ class GTSAM_EXPORT LinearSolver { } /** - * Factor method for generating a derived class of LinearSolver from + * Factory method for generating a derived class of LinearSolver from * LinearSolverParams * @param params LinearSolverParams linear optimizer parameters * @return pointer to a LinearSolver-derived object */ - static boost::shared_ptr FromLinearSolverParams( + static boost::shared_ptr CreateFromParameters( const LinearSolverParams ¶ms); }; diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index cc3932f2ff..31724032a6 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -60,7 +60,7 @@ TEST(LinearOptimizer, solverCheckIndividually) { // Multifrontal Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::MULTIFRONTAL_CHOLESKY; - auto solver = LinearSolver::FromLinearSolverParams(params); + auto solver = LinearSolver::CreateFromParameters(params); VectorValues actual = (*solver)(gfg); EXPECT(assert_equal(expected, actual)); actual = solver->solve(gfg); @@ -68,17 +68,17 @@ TEST(LinearOptimizer, solverCheckIndividually) { // Multifrontal QR, will be parallel if TBB installed params.linearSolverType = LinearSolverParams::MULTIFRONTAL_QR; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sequential Cholesky (more sensitive to conditioning, but faster) params.linearSolverType = LinearSolverParams::SEQUENTIAL_CHOLESKY; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sequential QR, not parallelized params.linearSolverType = LinearSolverParams::SEQUENTIAL_QR; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Iterative - either PCGSolver or SubgraphSolver @@ -87,18 +87,18 @@ TEST(LinearOptimizer, solverCheckIndividually) { LinearSolverParams::Iterative, Ordering::COLAMD, boost::make_shared( boost::make_shared())); - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // then SubgraphSolver params.ordering = Ordering::Colamd(gfg); params.iterativeParams = boost::make_shared(); - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // // cholmod - this flag exists for backwards compatibility but doesn't really // // correspond to any meaningful action // params.linearSolverType = LinearSolverParams::CHOLMOD; - // actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + // actual = LinearSolver::CreateFromParameters(params)->solve(gfg); // EXPECT(assert_equal(expected, actual)); // PCG - Preconditioned Conjugate Gradient, an iterative method @@ -106,37 +106,37 @@ TEST(LinearOptimizer, solverCheckIndividually) { LinearSolverParams::PCG, Ordering::COLAMD, boost::make_shared( boost::make_shared())); - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Subgraph - SPCG, see SubgraphSolver.h params.linearSolverType = LinearSolverParams::SUBGRAPH; params.ordering = Ordering::Colamd(gfg); params.iterativeParams = boost::make_shared(); - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sparse Eigen QR params.linearSolverType = LinearSolverParams::EIGEN_QR; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // Sparse Eigen Cholesky params.linearSolverType = LinearSolverParams::EIGEN_CHOLESKY; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); // SuiteSparse Cholesky #ifdef GTSAM_USE_SUITESPARSE params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif // CuSparse Cholesky #ifdef GTSAM_USE_CUSPARSE params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; - actual = LinearSolver::FromLinearSolverParams(params)->solve(gfg); + actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif } @@ -176,10 +176,10 @@ TEST(LinearOptimizer, solverCheckWithLoop) { #ifndef GTSAM_USE_CUSPARSE if (solverType == LSP::CUSPARSE_CHOLESKY) continue; #endif - auto params = LinearSolverParams(static_cast(solverType), - Ordering::Colamd(gfg), - createIterativeParams(solverType)); - auto linearSolver = LinearSolver::FromLinearSolverParams(params); + auto params = LinearSolverParams( + static_cast(solverType), Ordering::Colamd(gfg), + createIterativeParams(solverType)); + auto linearSolver = LinearSolver::CreateFromParameters(params); auto actual = (*linearSolver)(gfg); EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 44a84b5d97..27869b5352 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -134,7 +134,8 @@ VectorValues NonlinearOptimizer::solve( const GaussianFactorGraph& gfg, const NonlinearOptimizerParams& params) const { // solution of linear solver is an update to the linearization point - return LinearSolver::FromLinearSolverParams(params.linearSolverParams)->solve(gfg); + return LinearSolver::CreateFromParameters(params.linearSolverParams) + ->solve(gfg); } /* ************************************************************************* */ From 2dd1beef98c592fc70d496c1c3c72a4a3bb0717a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 01:08:16 -0500 Subject: [PATCH 84/91] Cholmod test - should throw error --- gtsam/linear/tests/testLinearSolver.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 31724032a6..50061e0317 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -95,11 +95,12 @@ TEST(LinearOptimizer, solverCheckIndividually) { actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); - // // cholmod - this flag exists for backwards compatibility but doesn't really - // // correspond to any meaningful action - // params.linearSolverType = LinearSolverParams::CHOLMOD; - // actual = LinearSolver::CreateFromParameters(params)->solve(gfg); - // EXPECT(assert_equal(expected, actual)); + // cholmod - this flag exists for backwards compatibility but doesn't really + // correspond to any meaningful action + // TODO: merge CHOLMOD and SuiteSparse ? + params.linearSolverType = LinearSolverParams::CHOLMOD; + THROWS_EXCEPTION(actual = + LinearSolver::CreateFromParameters(params)->solve(gfg);) // PCG - Preconditioned Conjugate Gradient, an iterative method params = LinearSolverParams( From 98262b32145c54df542da28be4137bc7d38d49ce Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 02:52:44 -0500 Subject: [PATCH 85/91] clean up LinearSolver interface * SuiteSparse & cuSPARSE can only do Cholesky - remove QR option * Iterative Solvers: construct from LinearSolverParams and check for null parameters in constructor * make_shared<> in LinearSolver::CreateFromParameters --- gtsam/linear/CuSparseSolver.cpp | 153 ++++++++++----------- gtsam/linear/CuSparseSolver.h | 10 +- gtsam/linear/IterativeSolver.h | 16 ++- gtsam/linear/LinearSolver.cpp | 41 ++---- gtsam/linear/LinearSolverParams.cpp | 45 +++--- gtsam/linear/LinearSolverParams.h | 57 ++++---- gtsam/linear/PCGSolver.cpp | 10 ++ gtsam/linear/PCGSolver.h | 4 +- gtsam/linear/SubgraphSolver.h | 14 ++ gtsam/linear/SuiteSparseSolver.cpp | 115 ++++++++-------- gtsam/linear/SuiteSparseSolver.h | 10 +- gtsam/linear/tests/testLinearSolver.cpp | 26 +++- gtsam/nonlinear/NonlinearOptimizerParams.h | 4 +- tests/testNonlinearOptimizer.cpp | 4 +- 14 files changed, 253 insertions(+), 256 deletions(-) diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index c3a20ad270..f329f33061 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -32,9 +32,8 @@ #include "gtsam/linear/SparseEigenSolver.h" namespace gtsam { - CuSparseSolver::CuSparseSolver(CuSparseSolver::CuSparseSolverType type, - const Ordering &ordering) - : solverType_(type), ordering_(ordering) {} + CuSparseSolver::CuSparseSolver(const Ordering &ordering) + : ordering_(ordering) {} #ifdef GTSAM_USE_CUSPARSE @@ -102,16 +101,13 @@ namespace gtsam { VectorValues CuSparseSolver::solve( const gtsam::GaussianFactorGraph &gfg) const { - if (solverType_ == QR) { - throw std::invalid_argument("This solver does not support QR."); - } else if (solverType_ == CHOLESKY) { - gttic_(CuSparseSolver_optimizeEigenCholesky); - - // ordering is used here - Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); - auto rows = Ab.rows(), cols = Ab.cols(); - Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); + gttic_(CuSparseSolver_optimizeEigenCholesky); + + // ordering is used here + Eigen::SparseMatrix + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); + auto rows = Ab.rows(), cols = Ab.cols(); + Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); // // // CSC in Eigen, CSR in CUDA, so A becomes At // int *At_row(NULL), *At_col(NULL); @@ -157,101 +153,98 @@ namespace gtsam { // // CHECK_CUSPARSE_ERROR(cusparseSetPointerMode(cspHandle, CUSPARSE_POINTER_MODE_HOST)); - //-------------------------------------------------------------------------- - // SpGEMM Computation + //-------------------------------------------------------------------------- + // SpGEMM Computation - auto At = A.transpose(); - Matrix b = At * Ab.col(cols - 1); + auto At = A.transpose(); + Matrix b = At * Ab.col(cols - 1); - Eigen::SparseMatrix - AtA(A.cols(), A.cols()); - AtA.selfadjointView().rankUpdate(At); - AtA.makeCompressed(); + Eigen::SparseMatrix + AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); + AtA.makeCompressed(); - gttic_(CuSparseSolver_optimizeEigenCholesky_solve); + gttic_(CuSparseSolver_optimizeEigenCholesky_solve); - // Create the cuSolver object - cusolverSpHandle_t solverHandle; - CHECK_CUSOLVER_ERROR(cusolverSpCreate(&solverHandle)); + // Create the cuSolver object + cusolverSpHandle_t solverHandle; + CHECK_CUSOLVER_ERROR(cusolverSpCreate(&solverHandle)); - // Create the matrix descriptor - cusparseMatDescr_t descrA; - CHECK_CUSPARSE_ERROR(cusparseCreateMatDescr(&descrA)); - CHECK_CUSPARSE_ERROR(cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL)); + // Create the matrix descriptor + cusparseMatDescr_t descrA; + CHECK_CUSPARSE_ERROR(cusparseCreateMatDescr(&descrA)); + CHECK_CUSPARSE_ERROR(cusparseSetMatType(descrA, CUSPARSE_MATRIX_TYPE_GENERAL)); - int *AtA_row(NULL), *AtA_col(NULL); - double *AtA_val(NULL); + int *AtA_row(NULL), *AtA_col(NULL); + double *AtA_val(NULL); - EigenSparseToCuSparseTranspose(AtA, &AtA_row, &AtA_col, &AtA_val); + EigenSparseToCuSparseTranspose(AtA, &AtA_row, &AtA_col, &AtA_val); // std::cout << "Base of AtA: " << AtA.outerIndexPtr()[0] << std::endl; - double *x_gpu(NULL), *b_gpu(NULL); + double *x_gpu(NULL), *b_gpu(NULL); - CHECK_CUDA_ERROR(cudaMalloc(&x_gpu, sizeof(double) * AtA.cols())); - CHECK_CUDA_ERROR(cudaMalloc(&b_gpu, sizeof(double) * AtA.cols())); + CHECK_CUDA_ERROR(cudaMalloc(&x_gpu, sizeof(double) * AtA.cols())); + CHECK_CUDA_ERROR(cudaMalloc(&b_gpu, sizeof(double) * AtA.cols())); - CHECK_CUDA_ERROR(cudaMemcpy(b_gpu, b.data(), - sizeof(double) * AtA.cols(), - cudaMemcpyHostToDevice)); + CHECK_CUDA_ERROR(cudaMemcpy(b_gpu, b.data(), + sizeof(double) * AtA.cols(), + cudaMemcpyHostToDevice)); - int singularity = 0; - const double tol = 0.00001; + int singularity = 0; + const double tol = 0.00001; - // no internal reordering, so only lower part (upper part of CSC) is used - CHECK_CUSOLVER_ERROR(cusolverSpDcsrlsvchol( - solverHandle, AtA.rows(), AtA.nonZeros(), descrA, - AtA_val, AtA_row, AtA_col, b_gpu, tol, 0, x_gpu, &singularity)); + // no internal reordering, so only lower part (upper part of CSC) is used + CHECK_CUSOLVER_ERROR(cusolverSpDcsrlsvchol( + solverHandle, AtA.rows(), AtA.nonZeros(), descrA, + AtA_val, AtA_row, AtA_col, b_gpu, tol, 0, x_gpu, &singularity)); - Vector x; - x.resize(A.cols()); - CHECK_CUDA_ERROR(cudaMemcpy(x.data(), x_gpu, sizeof(double) * A.cols(), - cudaMemcpyDeviceToHost)); + Vector x; + x.resize(A.cols()); + CHECK_CUDA_ERROR(cudaMemcpy(x.data(), x_gpu, sizeof(double) * A.cols(), + cudaMemcpyDeviceToHost)); - cudaFree(AtA_val); - cudaFree(AtA_row); - cudaFree(AtA_col); - cudaFree(b_gpu); - cudaFree(x_gpu); - cusolverSpDestroy(solverHandle); + cudaFree(AtA_val); + cudaFree(AtA_row); + cudaFree(AtA_col); + cudaFree(b_gpu); + cudaFree(x_gpu); + cusolverSpDestroy(solverHandle); - if (singularity != -1) - throw std::runtime_error(std::string("ILS in CUDA Solver, singularity: ") + std::to_string(singularity)); + if (singularity != -1) + throw std::runtime_error(std::string("ILS in CUDA Solver, singularity: ") + std::to_string(singularity)); - gttoc_(CuSparseSolver_optimizeEigenCholesky_solve); + gttoc_(CuSparseSolver_optimizeEigenCholesky_solve); - // NOTE: b is reordered now, so we need to transform back the order. - // First find dimensions of each variable - std::map dims; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) - continue; + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; - for (auto it = factor->begin(); it != factor->end(); ++it) { - dims[*it] = factor->getDim(it); - } + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); } + } - VectorValues vv; + VectorValues vv; - std::map columnIndices; + std::map columnIndices; - { - size_t currentColIndex = 0; - for (const auto key : ordering_) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } - } - - for (const std::pair keyDim : dims) { - vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + { + size_t currentColIndex = 0; + for (const auto key : ordering_) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; } + } - return vv; + for (const std::pair keyDim : dims) { + vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); } - throw std::exception(); + return vv; } #else VectorValues CuSparseSolver::solve( diff --git a/gtsam/linear/CuSparseSolver.h b/gtsam/linear/CuSparseSolver.h index 759a972e2c..9ba47b16f7 100644 --- a/gtsam/linear/CuSparseSolver.h +++ b/gtsam/linear/CuSparseSolver.h @@ -35,19 +35,11 @@ namespace gtsam { * cuSparse based Backend class */ class GTSAM_EXPORT CuSparseSolver : public LinearSolver { - public: - typedef enum { - QR, - CHOLESKY - } CuSparseSolverType; - protected: - CuSparseSolverType solverType_ = QR; Ordering ordering_; public: - explicit CuSparseSolver(CuSparseSolver::CuSparseSolverType type, - const Ordering &ordering); + explicit CuSparseSolver(const Ordering &ordering); /** Solves the GaussianFactorGraph using a sparse matrix solver * diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 5d15797559..dc428e9f26 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -93,19 +93,23 @@ class IterativeSolver : public LinearSolver { virtual ~IterativeSolver() { } - /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + /* interface to the nonlinear optimizer, without metadata, damping and initial + * estimate */ GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, boost::optional = boost::none, boost::optional&> lambda = boost::none) const; /* interface to the nonlinear optimizer, without initial estimate */ - GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, - const std::map &lambda) const; + GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda) const; - /* interface to the nonlinear optimizer that the subclasses have to implement */ + /* interface to the nonlinear optimizer that the subclasses have to implement + */ virtual VectorValues optimize(const GaussianFactorGraph &gfg, - const KeyInfo &keyInfo, const std::map &lambda, - const VectorValues &initial) const = 0; + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) const = 0; /* satisfies LinearSolver interface */ VectorValues solve(const GaussianFactorGraph &gfg) const override { diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index 1fdd5152f9..b05aafa564 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -37,40 +37,19 @@ boost::shared_ptr LinearSolver::CreateFromParameters( case LinearSolverParams::Iterative: return IterativeSolver::CreateFromParameters(params); case LinearSolverParams::PCG: - if (!params.iterativeParams) - throw std::runtime_error( - "LinearSolver::CreateFromParameters: iterative params has to be " - "assigned ..."); - return boost::make_shared( - *boost::static_pointer_cast( - params.iterativeParams)); + return boost::make_shared(params); case LinearSolverParams::SUBGRAPH: - if (!params.iterativeParams) - throw std::runtime_error( - "LinearSolver::CreateFromParameters: iterative params has to be " - "assigned ..."); - if (!params.ordering) - throw std::runtime_error( - "LinearSolver::CreateFromParameters: SubgraphSolver needs an " - "ordering"); - return boost::make_shared( - *boost::static_pointer_cast( - params.iterativeParams), - *params.ordering); + return boost::make_shared(params); case LinearSolverParams::EIGEN_QR: - return boost::shared_ptr(new SparseEigenSolver( - SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering)); + return boost::make_shared( + SparseEigenSolver::SparseEigenSolverType::QR, *params.ordering); case LinearSolverParams::EIGEN_CHOLESKY: - return boost::shared_ptr(new SparseEigenSolver( - SparseEigenSolver::SparseEigenSolverType::CHOLESKY, - *params.ordering)); - case LinearSolverParams::SUITESPARSE_CHOLESKY: - return boost::shared_ptr(new SuiteSparseSolver( - SuiteSparseSolver::SuiteSparseSolverType::CHOLESKY, - *params.ordering)); - case LinearSolverParams::CUSPARSE_CHOLESKY: - return boost::shared_ptr(new CuSparseSolver( - CuSparseSolver::CuSparseSolverType::CHOLESKY, *params.ordering)); + return boost::make_shared( + SparseEigenSolver::SparseEigenSolverType::CHOLESKY, *params.ordering); + case LinearSolverParams::SUITESPARSE: + return boost::make_shared(*params.ordering); + case LinearSolverParams::CUSPARSE: + return boost::make_shared(*params.ordering); case LinearSolverParams::CHOLMOD: default: throw std::runtime_error("Invalid parameters passed"); diff --git a/gtsam/linear/LinearSolverParams.cpp b/gtsam/linear/LinearSolverParams.cpp index 0aeb7a8a68..d5050525a2 100644 --- a/gtsam/linear/LinearSolverParams.cpp +++ b/gtsam/linear/LinearSolverParams.cpp @@ -27,8 +27,8 @@ void LinearSolverParams::setIterativeParams( /* ************************************************************************* */ -std::string LinearSolverParams::linearSolverTranslator( - LinearSolverType linearSolverType) const { +std::string LinearSolverParams::LinearSolverTranslator( + LinearSolverType linearSolverType) { switch (linearSolverType) { case MULTIFRONTAL_CHOLESKY: return "MULTIFRONTAL_CHOLESKY"; @@ -50,10 +50,10 @@ std::string LinearSolverParams::linearSolverTranslator( return "EIGEN_QR"; case EIGEN_CHOLESKY: return "EIGEN_CHOLESKY"; - case SUITESPARSE_CHOLESKY: - return "SUITESPARSE_CHOLESKY"; - case CUSPARSE_CHOLESKY: - return "CUSPARSE_CHOLESKY"; + case SUITESPARSE: + return "SUITESPARSE"; + case CUSPARSE: + return "CUSPARSE"; default: throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); @@ -61,8 +61,8 @@ std::string LinearSolverParams::linearSolverTranslator( } /* ************************************************************************* */ -LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( - const std::string &linearSolverType) const { +LinearSolverParams::LinearSolverType LinearSolverParams::LinearSolverTranslator( + const std::string &linearSolverType) { if (linearSolverType == "MULTIFRONTAL_CHOLESKY") return LinearSolverParams::MULTIFRONTAL_CHOLESKY; if (linearSolverType == "MULTIFRONTAL_QR") @@ -83,40 +83,41 @@ LinearSolverParams::LinearSolverType LinearSolverParams::linearSolverTranslator( return LinearSolverParams::EIGEN_CHOLESKY; if (linearSolverType == "EIGEN_QR") return LinearSolverParams::EIGEN_QR; - if (linearSolverType == "SUITESPARSE_CHOLESKY") - return LinearSolverParams::SUITESPARSE_CHOLESKY; - if (linearSolverType == "CUSPARSE_CHOLESKY") - return LinearSolverParams::CUSPARSE_CHOLESKY; + if (linearSolverType == "SUITESPARSE") + return LinearSolverParams::SUITESPARSE; + if (linearSolverType == "CUSPARSE") + return LinearSolverParams::CUSPARSE; throw std::invalid_argument( "Unknown linear solver type in SuccessiveLinearizationOptimizer"); } /* ************************************************************************* */ -std::string LinearSolverParams::orderingTypeTranslator( - Ordering::OrderingType type) const { +std::string LinearSolverParams::OrderingTypeTranslator( + Ordering::OrderingType type) { switch (type) { case Ordering::METIS: return "METIS"; case Ordering::COLAMD: return "COLAMD"; + case Ordering::CUSTOM: + return "CUSTOM"; default: - if (ordering) - return "CUSTOM"; - else - throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + throw std::invalid_argument( + "Invalid ordering type: see setOrdering or setOrderingType"); } } /* ************************************************************************* */ -Ordering::OrderingType LinearSolverParams::orderingTypeTranslator( - const std::string &type) const { +Ordering::OrderingType LinearSolverParams::OrderingTypeTranslator( + const std::string &type) { if (type == "METIS") return Ordering::METIS; if (type == "COLAMD") return Ordering::COLAMD; + if (type == "CUSTOM") + return Ordering::CUSTOM; throw std::invalid_argument( - "Invalid ordering type: You must provide an ordering for a custom ordering type. See setOrdering"); + "Invalid ordering type: see setOrdering or setOrderingType"); } } diff --git a/gtsam/linear/LinearSolverParams.h b/gtsam/linear/LinearSolverParams.h index 8ab8f06992..daee02961f 100644 --- a/gtsam/linear/LinearSolverParams.h +++ b/gtsam/linear/LinearSolverParams.h @@ -38,17 +38,26 @@ struct GTSAM_EXPORT LinearSolverParams { Iterative, /* Experimental Flag - donotuse: for backwards compatibility only. Use PCG or SUBGRAPH instead */ CHOLMOD, /* Experimental Flag - donotuse: for backwards compatibility. use - SUITESPARSE_CHOLESKY or PCG/SUBGRAPH w/ iterativeParams instead - */ + SUITESPARSE or PCG/SUBGRAPH w/ iterativeParams instead */ PCG, SUBGRAPH, EIGEN_QR, EIGEN_CHOLESKY, - SUITESPARSE_CHOLESKY, - CUSPARSE_CHOLESKY, + SUITESPARSE, + CUSPARSE, LAST /* for iterating over enum */ } LinearSolverType; + /// The type of linear solver to use in the nonlinear optimizer + /// (default: MULTIFRONTAL_CHOLESKY) + LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; + /// The method of ordering use during variable elimination (default: COLAMD) + Ordering::OrderingType orderingType = Ordering::COLAMD; + /// The variable elimination ordering, or empty to use COLAMD (default: empty) + boost::optional ordering; + /// The container for iterativeOptimization parameters. used in CG Solvers. + boost::shared_ptr iterativeParams; + /// Construct a params object from the solver and ordering types LinearSolverParams(LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY, Ordering::OrderingType orderingType = Ordering::COLAMD) @@ -80,17 +89,6 @@ struct GTSAM_EXPORT LinearSolverParams { ordering(ordering), iterativeParams(iterativeParams) {}; - /// The type of linear solver to use in the nonlinear optimizer - /// (default: MULTIFRONTAL_CHOLESKY) - LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY; - /// The method of ordering use during variable elimination (default: COLAMD) - Ordering::OrderingType orderingType = Ordering::COLAMD; - /// The variable elimination ordering, or empty to use COLAMD (default: empty) - boost::optional ordering; - - /// The container for iterativeOptimization parameters. used in CG Solvers. - boost::shared_ptr iterativeParams; - inline bool isMultifrontal() const { return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); @@ -118,12 +116,12 @@ struct GTSAM_EXPORT LinearSolverParams { return (linearSolverType == EIGEN_CHOLESKY); } - inline bool isSuiteSparseCholesky() const { - return (linearSolverType == SUITESPARSE_CHOLESKY); + inline bool isSuiteSparse() const { + return (linearSolverType == SUITESPARSE); } - inline bool isCuSparseCholesky() const { - return (linearSolverType == CUSPARSE_CHOLESKY); + inline bool isCuSparse() const { + return (linearSolverType == CUSPARSE); } GaussianFactorGraph::Eliminate getEliminationFunction() const { @@ -143,11 +141,11 @@ struct GTSAM_EXPORT LinearSolverParams { } std::string getLinearSolverType() const { - return linearSolverTranslator(linearSolverType); + return LinearSolverTranslator(linearSolverType); } void setLinearSolverType(const std::string& solver) { - linearSolverType = linearSolverTranslator(solver); + linearSolverType = LinearSolverTranslator(solver); } void setIterativeParams(const boost::shared_ptr params); @@ -158,12 +156,13 @@ struct GTSAM_EXPORT LinearSolverParams { } std::string getOrderingType() const { - return orderingTypeTranslator(orderingType); + return OrderingTypeTranslator(orderingType); } - // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type + // Note that if you want to use a custom ordering, you must set the ordering + // directly, this will switch to custom type void setOrderingType(const std::string& ordering){ - orderingType = orderingTypeTranslator(ordering); + orderingType = OrderingTypeTranslator(ordering); } private: @@ -174,10 +173,12 @@ struct GTSAM_EXPORT LinearSolverParams { * "METIS" <-> Ordering::METIS * @{ */ - std::string linearSolverTranslator(LinearSolverType linearSolverType) const; - LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const; - std::string orderingTypeTranslator(Ordering::OrderingType type) const; - Ordering::OrderingType orderingTypeTranslator(const std::string& type) const; + static std::string LinearSolverTranslator( + const LinearSolverType linearSolverType); + static LinearSolverType LinearSolverTranslator( + const std::string& linearSolverType); + static std::string OrderingTypeTranslator(const Ordering::OrderingType type); + static Ordering::OrderingType OrderingTypeTranslator(const std::string& type); /**@}*/ }; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 00dc946e38..e0aae5224c 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -45,6 +45,16 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) : parameters_(p), preconditioner_(createPreconditioner(p.preconditioner_)) {} +PCGSolver::PCGSolver(const LinearSolverParams ¶ms) { + if (!params.iterativeParams) + throw std::runtime_error( + "LinearSolver::CreateFromParameters: iterative params has to be " + "assigned ..."); + parameters_ = + *boost::static_pointer_cast(params.iterativeParams); + preconditioner_ = createPreconditioner(parameters_.preconditioner_); +} + void PCGSolverParameters::setPreconditionerParams(const boost::shared_ptr preconditioner) { preconditioner_ = preconditioner; } diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h index f9eb5cc6ad..a3cd0c036e 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -74,8 +74,8 @@ class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: /* Interface to initialize a solver without a problem */ PCGSolver(const PCGSolverParameters &p); - virtual ~PCGSolver() { - } + PCGSolver(const LinearSolverParams &p); + virtual ~PCGSolver() {} using IterativeSolver::optimize; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index eb51b4ec45..61bc5349b9 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -37,6 +37,7 @@ class SubgraphPreconditioner; struct GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { + typedef boost::shared_ptr shared_ptr; SubgraphBuilderParameters builderParams; explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) : builderParams(p) {} @@ -153,6 +154,19 @@ class GTSAM_EXPORT SubgraphSolverWrapper : public LinearSolver { SubgraphSolverWrapper(const SubgraphSolverParameters ¶meters, const Ordering &ordering) : parameters_(parameters), ordering_(ordering) {}; + SubgraphSolverWrapper(const LinearSolverParams ¶ms) { + if (!params.iterativeParams) + throw std::runtime_error( + "SubgraphSolverWrapper::SubgraphSolverWrapper: iterative params has " + "to be assigned ..."); + if (!params.ordering) + throw std::runtime_error( + "SubgraphSolverWrapper::SubgraphSolverWrapper: SubgraphSolver needs " + "an ordering"); + parameters_ = *boost::static_pointer_cast( + params.iterativeParams); + ordering_ = *params.ordering; + }; /// satisfies LinearSolver interface to solve the GaussianFactorGraph. VectorValues solve(const GaussianFactorGraph &gfg) const override { diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index 4a1030fdb6..cbb7214c53 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -27,79 +27,72 @@ #endif namespace gtsam { - SuiteSparseSolver::SuiteSparseSolver( - SuiteSparseSolver::SuiteSparseSolverType type, const Ordering &ordering) - : solverType_(type), ordering_(ordering) {} + SuiteSparseSolver::SuiteSparseSolver(const Ordering &ordering) + : ordering_(ordering) {} #ifdef GTSAM_USE_SUITESPARSE VectorValues SuiteSparseSolver::solve( const gtsam::GaussianFactorGraph &gfg) const { - if (solverType_ == QR) { - throw std::invalid_argument("This solver does not support QR."); - } else if (solverType_ == CHOLESKY) { - gttic_(SuiteSparseSolver_optimizeEigenCholesky); - - // this is where ordering is used - Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); - auto rows = Ab.rows(), cols = Ab.cols(); - auto A = Ab.block(0, 0, rows, cols - 1); - auto At = A.transpose(); - auto b = Ab.col(cols - 1); - - Eigen::SparseMatrix - AtA(A.cols(), A.cols()); - AtA.selfadjointView().rankUpdate(At); - - gttic_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); - // Solve A*x = b using sparse Cholesky from Eigen - Eigen::CholmodSupernodalLLT - , Eigen::Upper> - solver; - solver.cholmod().nmethods = 1; - solver.cholmod().method[0].ordering = CHOLMOD_NATURAL; - solver.cholmod().postorder = false; - - solver.compute(AtA); - gttoc_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); - - gttic_(SuiteSparseSolver_optimizeEigenCholesky_solve); - Matrix Atb = (At * b).eval(); - Eigen::VectorXd x = solver.solve(Atb); - gttoc_(SuiteSparseSolver_optimizeEigenCholesky_solve); - - // NOTE: b is reordered now, so we need to transform back the order. - // First find dimensions of each variable - std::map dims; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) - continue; - - for (auto it = factor->begin(); it != factor->end(); ++it) { - dims[*it] = factor->getDim(it); - } + gttic_(SuiteSparseSolver_optimizeEigenCholesky); + + // this is where ordering is used + Eigen::SparseMatrix + Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); + auto rows = Ab.rows(), cols = Ab.cols(); + auto A = Ab.block(0, 0, rows, cols - 1); + auto At = A.transpose(); + auto b = Ab.col(cols - 1); + + Eigen::SparseMatrix + AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); + + gttic_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); + // Solve A*x = b using sparse Cholesky from Eigen + Eigen::CholmodSupernodalLLT + , Eigen::Upper> + solver; + solver.cholmod().nmethods = 1; + solver.cholmod().method[0].ordering = CHOLMOD_NATURAL; + solver.cholmod().postorder = false; + + solver.compute(AtA); + gttoc_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); + + gttic_(SuiteSparseSolver_optimizeEigenCholesky_solve); + Matrix Atb = (At * b).eval(); + Eigen::VectorXd x = solver.solve(Atb); + gttoc_(SuiteSparseSolver_optimizeEigenCholesky_solve); + + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) + continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); } + } - VectorValues vv; - - std::map columnIndices; + VectorValues vv; - { - size_t currentColIndex = 0; - for (const auto key : ordering_) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } - } + std::map columnIndices; - for (const std::pair keyDim : dims) { - vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + { + size_t currentColIndex = 0; + for (const auto key : ordering_) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; } + } - return vv; + for (const std::pair keyDim : dims) { + vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); } - throw std::exception(); + return vv; } #else VectorValues SuiteSparseSolver::solve( diff --git a/gtsam/linear/SuiteSparseSolver.h b/gtsam/linear/SuiteSparseSolver.h index 6866ccbf66..fdbb5bad48 100644 --- a/gtsam/linear/SuiteSparseSolver.h +++ b/gtsam/linear/SuiteSparseSolver.h @@ -35,19 +35,11 @@ namespace gtsam { * Eigen SparseSolver based Backend class */ class GTSAM_EXPORT SuiteSparseSolver : public LinearSolver { - public: - typedef enum { - QR, - CHOLESKY - } SuiteSparseSolverType; - protected: - SuiteSparseSolverType solverType_ = QR; Ordering ordering_; public: - explicit SuiteSparseSolver(SuiteSparseSolver::SuiteSparseSolverType type, - const Ordering &ordering); + explicit SuiteSparseSolver(const Ordering &ordering); /** Solves the GaussianFactorGraph using a sparse matrix solver * diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index 50061e0317..e86a560b43 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -129,14 +129,14 @@ TEST(LinearOptimizer, solverCheckIndividually) { // SuiteSparse Cholesky #ifdef GTSAM_USE_SUITESPARSE - params.linearSolverType = LinearSolverParams::SUITESPARSE_CHOLESKY; + params.linearSolverType = LinearSolverParams::SUITESPARSE; actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif // CuSparse Cholesky #ifdef GTSAM_USE_CUSPARSE - params.linearSolverType = LinearSolverParams::CUSPARSE_CHOLESKY; + params.linearSolverType = LinearSolverParams::CUSPARSE; actual = LinearSolver::CreateFromParameters(params)->solve(gfg); EXPECT(assert_equal(expected, actual)); #endif @@ -172,10 +172,10 @@ TEST(LinearOptimizer, solverCheckWithLoop) { solverType++) { if (solverType == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option #ifndef GTSAM_USE_SUITESPARSE - if (solverType == LSP::SUITESPARSE_CHOLESKY) continue; + if (solverType == LSP::SUITESPARSE) continue; #endif #ifndef GTSAM_USE_CUSPARSE - if (solverType == LSP::CUSPARSE_CHOLESKY) continue; + if (solverType == LSP::CUSPARSE) continue; #endif auto params = LinearSolverParams( static_cast(solverType), Ordering::Colamd(gfg), @@ -186,6 +186,24 @@ TEST(LinearOptimizer, solverCheckWithLoop) { } } +/* ************************************************************************* */ +// assert Iterative, PCG, and Subgraph will throw errors if iterativeParams not +// set +TEST(LinearOptimizer, IterativeThrowError) { + LinearSolverParams params; + params.orderingType = Ordering::COLAMD; + + params.linearSolverType = LinearSolverParams::Iterative; + CHECK_EXCEPTION(LinearSolver::CreateFromParameters(params), + std::runtime_error) + params.linearSolverType = LinearSolverParams::PCG; + CHECK_EXCEPTION(LinearSolver::CreateFromParameters(params), + std::runtime_error) + params.linearSolverType = LinearSolverParams::SUBGRAPH; + CHECK_EXCEPTION(LinearSolver::CreateFromParameters(params), + std::runtime_error) +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 593b1d8344..f941531a96 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -134,8 +134,8 @@ class GTSAM_EXPORT NonlinearOptimizerParams { static constexpr LinearSolverType SUBGRAPH = LinearSolverParams::SUBGRAPH; static constexpr LinearSolverType EIGEN_QR = LinearSolverParams::EIGEN_QR; static constexpr LinearSolverType EIGEN_CHOLESKY = LinearSolverParams::EIGEN_CHOLESKY; - static constexpr LinearSolverType SUITESPARSE_CHOLESKY = LinearSolverParams::SUITESPARSE_CHOLESKY; - static constexpr LinearSolverType CUSPARSE_CHOLESKY = LinearSolverParams::CUSPARSE_CHOLESKY; + static constexpr LinearSolverType SUITESPARSE = LinearSolverParams::SUITESPARSE; + static constexpr LinearSolverType CUSPARSE = LinearSolverParams::CUSPARSE; static constexpr LinearSolverType LAST = LinearSolverParams::LAST; /// The type of linear solver to use in the nonlinear optimizer diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index bee5d193ff..41433c623d 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -182,10 +182,10 @@ TEST(NonlinearOptimizer, optimization_method) { for (int solver = LSP::MULTIFRONTAL_CHOLESKY; solver != LSP::LAST; solver++) { if (solver == LSP::CHOLMOD) continue; // CHOLMOD is an undefined option #ifndef GTSAM_USE_SUITESPARSE - if (solver == LSP::SUITESPARSE_CHOLESKY) continue; + if (solver == LSP::SUITESPARSE) continue; #endif #ifndef GTSAM_USE_CUSPARSE - if (solver == LSP::CUSPARSE_CHOLESKY) continue; + if (solver == LSP::CUSPARSE) continue; #endif params.linearSolverType = static_cast(solver); params.iterativeParams = createIterativeParams(solver); From d9958d45d60dc80a1a787c0f26b2885f13970102 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 03:30:41 -0500 Subject: [PATCH 86/91] split SparseEigen Solve --- gtsam/linear/SparseEigenSolver.cpp | 130 ++++++++++++++++------------- gtsam/linear/SparseEigenSolver.h | 3 +- 2 files changed, 72 insertions(+), 61 deletions(-) diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 288f0065bc..9001728c2f 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -146,80 +146,90 @@ namespace gtsam { Ab.col(cols)); } - VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) const { - if (solverType_ == QR) { - gttic_(EigenOptimizer_optimizeEigenQR); + // Solves using sparse QR (used when solverType_ == QR) + VectorValues solveQr(const GaussianFactorGraph &gfg, + const Ordering &ordering) { + gttic_(EigenOptimizer_optimizeEigenQR); - // this is where ordering is used - auto Ab_pair = obtainSparseMatrix(gfg, ordering_); + // this is where ordering is used + auto Ab_pair = obtainSparseMatrix(gfg, ordering); - // Solve A*x = b using sparse QR from Eigen - gttic_(EigenOptimizer_optimizeEigenQR_create_solver); - Eigen::SparseQR> solver(Ab_pair.first); - gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); + // Solve A*x = b using sparse QR from Eigen + gttic_(EigenOptimizer_optimizeEigenQR_create_solver); + Eigen::SparseQR> solver(Ab_pair.first); + gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); - gttic_(EigenOptimizer_optimizeEigenQR_solve); - Eigen::VectorXd x = solver.solve(Ab_pair.second); - gttoc_(EigenOptimizer_optimizeEigenQR_solve); + gttic_(EigenOptimizer_optimizeEigenQR_solve); + Eigen::VectorXd x = solver.solve(Ab_pair.second); + gttoc_(EigenOptimizer_optimizeEigenQR_solve); - return VectorValues(x, gfg.getKeyDimMap()); - } else if (solverType_ == CHOLESKY) { - gttic_(EigenOptimizer_optimizeEigenCholesky); - SpMat Ab = sparseJacobianEigen(gfg, ordering_); - auto rows = Ab.rows(), cols = Ab.cols(); - auto A = Ab.block(0, 0, rows, cols - 1); - auto At = A.transpose(); - auto b = Ab.col(cols - 1); - - SpMat AtA(A.cols(), A.cols()); - AtA.selfadjointView().rankUpdate(At); - - gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); - // Solve A*x = b using sparse Cholesky from Eigen - Eigen::SimplicialLDLT> - solver(AtA); - - gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); - - gttic_(EigenOptimizer_optimizeEigenCholesky_solve); - Eigen::VectorXd x = solver.solve(At * b); - gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); - - // NOTE: b is reordered now, so we need to transform back the order. - // First find dimensions of each variable - std::map dims; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) - continue; - - for (auto it = factor->begin(); it != factor->end(); ++it) { - dims[*it] = factor->getDim(it); - } - } + return VectorValues(x, gfg.getKeyDimMap()); + } + + // Solves using sparse Cholesky (used when solverType_ == CHOLESKY) + VectorValues solveCholesky(const GaussianFactorGraph &gfg, + const Ordering &ordering) { + gttic_(EigenOptimizer_optimizeEigenCholesky); + SpMat Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); + auto rows = Ab.rows(), cols = Ab.cols(); + auto A = Ab.block(0, 0, rows, cols - 1); + auto At = A.transpose(); + auto b = Ab.col(cols - 1); + + SpMat AtA(A.cols(), A.cols()); + AtA.selfadjointView().rankUpdate(At); - VectorValues vv; + gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); + // Solve A*x = b using sparse Cholesky from Eigen + Eigen::SimplicialLDLT> + solver(AtA); - std::map columnIndices; + gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); - { - size_t currentColIndex = 0; - for (const auto key : ordering_) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } + gttic_(EigenOptimizer_optimizeEigenCholesky_solve); + Eigen::VectorXd x = solver.solve(At * b); + gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); + + // NOTE: b is reordered now, so we need to transform back the order. + // First find dimensions of each variable + std::map dims; + for (const boost::shared_ptr &factor : gfg) { + if (!static_cast(factor)) continue; + + for (auto it = factor->begin(); it != factor->end(); ++it) { + dims[*it] = factor->getDim(it); } + } - for (const pair keyDim : dims) { - vv.insert(keyDim.first, x.segment(columnIndices[keyDim.first], keyDim.second)); + VectorValues vv; + + std::map columnIndices; + + { + size_t currentColIndex = 0; + for (const auto key : ordering) { + columnIndices[key] = currentColIndex; + currentColIndex += dims[key]; } + } - return vv; + for (const pair keyDim : dims) { + vv.insert(keyDim.first, + x.segment(columnIndices[keyDim.first], keyDim.second)); + } + + return vv; + } + + VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) const { + if (solverType_ == QR) { + return solveQr(gfg, ordering_); + } else if (solverType_ == CHOLESKY) { + return solveCholesky(gfg, ordering_); } throw std::exception(); } - SparseEigenSolver::SparseEigenSolver( - SparseEigenSolver::SparseEigenSolverType type, const Ordering &ordering) - : solverType_(type), ordering_(ordering) {} } // namespace gtsam diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index be305654fe..14f5240eb9 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -50,7 +50,8 @@ class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { public: explicit SparseEigenSolver(SparseEigenSolver::SparseEigenSolverType type, - const Ordering &ordering); + const Ordering &ordering) + : solverType_(type), ordering_(ordering) {} /** Solves the GaussianFactorGraph using a sparse matrix solver * From 216e1dfa6f0853bfc7cffa22d668f0491c36b361 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 03:31:05 -0500 Subject: [PATCH 87/91] Add authors --- gtsam/linear/LinearSolver.cpp | 2 +- gtsam/linear/LinearSolver.h | 2 +- gtsam/linear/tests/testLinearSolver.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/LinearSolver.cpp b/gtsam/linear/LinearSolver.cpp index b05aafa564..b0c4d74c8d 100644 --- a/gtsam/linear/LinearSolver.cpp +++ b/gtsam/linear/LinearSolver.cpp @@ -12,7 +12,7 @@ /** * @file LinearSolver.cpp * @brief Common Interface for Linear Solvers - * @author Fan Jiang + * @author Fan Jiang, Gerry Chen, Mandy Xie, and Frank Dellaert */ #include diff --git a/gtsam/linear/LinearSolver.h b/gtsam/linear/LinearSolver.h index d1b6e28547..a243c332c7 100644 --- a/gtsam/linear/LinearSolver.h +++ b/gtsam/linear/LinearSolver.h @@ -12,7 +12,7 @@ /** * @file LinearSolver.h * @brief Common Interface for Linear Solvers - * @author Fan Jiang + * @author Fan Jiang, Gerry Chen, Mandy Xie, and Frank Dellaert */ #pragma once diff --git a/gtsam/linear/tests/testLinearSolver.cpp b/gtsam/linear/tests/testLinearSolver.cpp index e86a560b43..48c0404fc6 100644 --- a/gtsam/linear/tests/testLinearSolver.cpp +++ b/gtsam/linear/tests/testLinearSolver.cpp @@ -12,7 +12,7 @@ /** * @file testLinearSolver.cpp * @brief Tests for Common Interface for Linear Solvers - * @author Fan Jiang + * @author Fan Jiang and Gerry Chen */ #include From 51c150367139f67f351864a31d5c483a814080ee Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 03:35:16 -0500 Subject: [PATCH 88/91] undo unnecessary change due to imperfect merge --- gtsam/linear/Scatter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index 68408e99be..b05d191bc9 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -58,7 +58,7 @@ class Scatter : public FastVector { GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering); /// Add a key/dim pair - void add(Key key, size_t dim); + GTSAM_EXPORT void add(Key key, size_t dim); private: /// Find the SlotEntry with the right key (linear time worst case) From 13437c6b59bd073d2dc0b7791177ff2aaeaf9e2d Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 10:21:17 -0500 Subject: [PATCH 89/91] Organize Sparse Jacobian functions to be only in GaussianFactorGraph sparseJacobian is now templated on its return type for more flexibility --- gtsam/base/Matrix.h | 11 ++ gtsam/linear/CuSparseSolver.cpp | 18 +- gtsam/linear/GaussianFactorGraph.cpp | 95 +++++++---- gtsam/linear/GaussianFactorGraph.h | 53 +++--- gtsam/linear/SparseEigenSolver.cpp | 156 +++--------------- gtsam/linear/SparseEigenSolver.h | 6 +- gtsam/linear/SuiteSparseSolver.cpp | 18 +- .../linear/tests/testGaussianFactorGraph.cpp | 103 ++++++++++-- 8 files changed, 233 insertions(+), 227 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 071c33fca0..0d1cf85425 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -28,11 +28,15 @@ #include #include +#include + #include #include #include #include +#include + /** * Matrix is a typedef in the gtsam namespace * TODO: make a version to work with matlab wrapping @@ -73,6 +77,13 @@ GTSAM_MAKE_MATRIX_DEFS(9); typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; +// Sparse Matrix Formats +typedef std::vector> + SparseMatrixBoostTriplets; +typedef std::vector> SparseMatrixEigenTriplets; +typedef Eigen::SparseMatrix + SparseMatrixEigen; + // Matrix formatting arguments when printing. // Akin to Matlab style. const Eigen::IOFormat& matlabFormat(); diff --git a/gtsam/linear/CuSparseSolver.cpp b/gtsam/linear/CuSparseSolver.cpp index f329f33061..9812696b5e 100644 --- a/gtsam/linear/CuSparseSolver.cpp +++ b/gtsam/linear/CuSparseSolver.cpp @@ -29,7 +29,7 @@ #endif -#include "gtsam/linear/SparseEigenSolver.h" +#include namespace gtsam { CuSparseSolver::CuSparseSolver(const Ordering &ordering) @@ -65,9 +65,8 @@ namespace gtsam { #define CHECK_CUSPARSE_ERROR(code) checkCuSparseError(code, ____LOCATION) - void EigenSparseToCuSparseTranspose( - const Eigen::SparseMatrix &mat, int **row, int **col, double **val) - { + void EigenSparseToCuSparseTranspose(const SparseMatrixEigen &mat, int **row, + int **col, double **val) { const int num_non0 = mat.nonZeros(); const int num_outer = mat.cols() + 1; @@ -104,10 +103,10 @@ namespace gtsam { gttic_(CuSparseSolver_optimizeEigenCholesky); // ordering is used here - Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); - auto rows = Ab.rows(), cols = Ab.cols(); - Eigen::SparseMatrix A = Ab.block(0, 0, rows, cols - 1); + size_t rows, cols; + SparseMatrixEigen Ab; + std::tie(rows, cols, Ab) = gfg.sparseJacobian(ordering_); + SparseMatrixEigen A = Ab.block(0, 0, rows, cols - 1); // // // CSC in Eigen, CSR in CUDA, so A becomes At // int *At_row(NULL), *At_col(NULL); @@ -159,8 +158,7 @@ namespace gtsam { auto At = A.transpose(); Matrix b = At * Ab.col(cols - 1); - Eigen::SparseMatrix - AtA(A.cols(), A.cols()); + SparseMatrixEigen AtA(A.cols(), A.cols()); AtA.selfadjointView().rankUpdate(At); AtA.makeCompressed(); diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 83cd2b7ae6..ef85f5f381 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -100,16 +100,66 @@ namespace gtsam { } /* ************************************************************************* */ - vector > - GaussianFactorGraph::sparseJacobian() const { - Ordering ord(this->keys()); + /// instantiate explicit template functions with sparse matrix representations + // Boost Tuples + template std::tuple + GaussianFactorGraph::sparseJacobian( + const Ordering& ordering) const; + // Eigen Triplets + template std::tuple + GaussianFactorGraph::sparseJacobian( + const Ordering& ordering) const; + // Eigen Sparse Matrix (template specialized) + template <> + std::tuple + GaussianFactorGraph::sparseJacobian( + const Ordering& ordering) const { + gttic_(GaussianFactorGraph_sparseJacobian); + gttic_(obtainSparseJacobian); + size_t rows, cols; + SparseMatrixEigenTriplets entries; + std::tie(rows, cols, entries) = + sparseJacobian(ordering); + gttoc_(obtainSparseJacobian); + gttic_(convertSparseJacobian); + SparseMatrixEigen Ab(rows, cols); + Ab.reserve(entries.size()); + for (auto entry : entries) { + Ab.insert(entry.row(), entry.col()) = entry.value(); + } + Ab.makeCompressed(); + // TODO(gerry): benchmark to see if setFromTriplets is faster + // Ab.setFromTriplets(entries.begin(), entries.end()); + return std::make_tuple(rows, cols, Ab); + } + // Eigen Matrix in "matlab" format (template specialized) + template <> + std::tuple + GaussianFactorGraph::sparseJacobian(const Ordering& ordering) const { + gttic_(GaussianFactorGraph_sparseJacobian); + // call sparseJacobian + size_t nrows, ncols; + SparseMatrixBoostTriplets result; + std::tie(nrows, ncols, result) = + sparseJacobian(ordering); - return sparseJacobian(ord); + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3, nzmax); + for (size_t k = 0; k < result.size(); k++) { + const auto& entry = result[k]; + IJS(0, k) = double(entry.get<0>() + 1); + IJS(1, k) = double(entry.get<1>() + 1); + IJS(2, k) = entry.get<2>(); + } + return std::make_tuple(nrows, ncols, IJS); } /* ************************************************************************* */ - vector > - GaussianFactorGraph::sparseJacobian( + // sparse matrix representation template `XXXEntries` must satisfy + // `XXXEntries.emplace_back(int i, int j, double s)` to compile + template + std::tuple GaussianFactorGraph::sparseJacobian( const Ordering& ordering) const { gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable @@ -132,8 +182,9 @@ namespace gtsam { } // Iterate over all factors, adding sparse scalar entries - typedef boost::tuple triplet; - vector entries; + Entries entries; + entries.reserve(60 * size()); + size_t row = 0; for (const sharedFactor& factor : *this) { if (!static_cast(factor)) continue; @@ -178,34 +229,8 @@ namespace gtsam { // Increment row index row += jacobianFactor->rows(); } - - return entries; - } - /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparseJacobian_() const { - Ordering ord = Ordering::Natural(*this); - - return sparseJacobian_(ord); - } - - /* ************************************************************************* */ - Matrix GaussianFactorGraph::sparseJacobian_( - const Ordering& ordering) const { - // call sparseJacobian - typedef boost::tuple triplet; - vector result = sparseJacobian(ordering); - - // translate to base 1 matrix - size_t nzmax = result.size(); - Matrix IJS(3,nzmax); - for (size_t k = 0; k < result.size(); k++) { - const triplet& entry = result[k]; - IJS(0,k) = double(entry.get<0>() + 1); - IJS(1,k) = double(entry.get<1>() + 1); - IJS(2,k) = entry.get<2>(); - } - return IJS; + return std::make_tuple(row, currentColIndex+1, entries); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 78a957cc2f..8bd18b3cb6 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -181,34 +181,47 @@ namespace gtsam { ///@{ /** - * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, - * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. + * Generates an m-by-n sparse Jacobian matrix, where i(k) and j(k) are the + * base 0 row and column indices, s(k) a double. Column ordering is taken as + * default. The return type is a sparse matrix representation templated + * type which may be: + * 1. vector> is a vector of 3-tuples + * (i, j, s) + * 2. vector> is a vector of Eigen-triples (i, j, s) + * 3. Eigen::SparseMatrix is an Eigen format sparse matrix + * 4. Matrix (i/j will be 1-indexed instead of 0-indexed, for matlab) is a + * 3xK matrix [I;J;S] * The standard deviations are baked into A and b + * @return 3-tuple with the dimensions of the Jacobian as the first 2 + * elements and the sparse matrix in one of the 4 form above as the 3rd + * element. */ - std::vector > sparseJacobian() const; + template + std::tuple sparseJacobian() const { + Ordering ord(this->keys()); + return sparseJacobian(ord); + } /** - * Return vector of i, j, and s to generate an m-by-n sparse Jacobian matrix, - * where i(k) and j(k) are the base 0 row and column indices, s(k) a double. + * Generates an m-by-n sparse Jacobian matrix, where i(k) and j(k) are the + * base 0 row and column indices, s(k) a double. The return type is a sparse + * matrix representation templated type which may be: + * 1. vector> is a vector of 3-tuples + * (i, j, s) + * 2. vector> is a vector of Eigen-triples (i, j, s) + * 3. Eigen::SparseMatrix is an Eigen format sparse matrix + * 4. Matrix (i/j will be 1-indexed instead of 0-indexed, for matlab) is a + * 3xK matrix [I;J;S] * The standard deviations are baked into A and b + * @param ordering the column ordering + * @return 3-tuple with the dimensions of the Jacobian as the first 2 + * elements and the sparse matrix in one of the 4 form above as the 3rd + * element. */ - std::vector > sparseJacobian( + template + std::tuple sparseJacobian( const Ordering& ordering) const; - /** - * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * The standard deviations are baked into A and b - */ - Matrix sparseJacobian_() const; - - /** - * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] entries - * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. - * The standard deviations are baked into A and b - */ - Matrix sparseJacobian_(const Ordering& ordering) const; - /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ * Jacobian matrix, augmented with b with the noise models baked diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index 9001728c2f..ada6dcd011 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -18,6 +18,7 @@ * @author Mandy Xie * @author Fan Jiang * @author Frank Dellaert + * @author Gerry Chen */ #include @@ -31,137 +32,25 @@ using namespace std; namespace gtsam { - using SpMat = Eigen::SparseMatrix; - - Eigen::SparseMatrix - SparseEigenSolver::sparseJacobianEigen( - const GaussianFactorGraph &gfg, - const Ordering &ordering) { - // First find dimensions of each variable - std::map dims; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) - continue; - - for (auto it = factor->begin(); it != factor->end(); ++it) { - dims[*it] = factor->getDim(it); - } - } - - // Compute first scalar column of each variable - size_t currentColIndex = 0; - std::map columnIndices; - for (const auto key : ordering) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; - } - - // Iterate over all factors, adding sparse scalar entries - vector> entries; - entries.reserve(60 * gfg.size()); - - size_t row = 0; - for (const boost::shared_ptr &factor : gfg) { - if (!static_cast(factor)) continue; - - // Convert to JacobianFactor if necessary - JacobianFactor::shared_ptr jacobianFactor( - boost::dynamic_pointer_cast(factor)); - if (!jacobianFactor) { - HessianFactor::shared_ptr hessian( - boost::dynamic_pointer_cast(factor)); - if (hessian) - jacobianFactor.reset(new JacobianFactor(*hessian)); - else - throw invalid_argument( - "GaussianFactorGraph contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - - // Whiten the factor and add entries for it - // iterate over all variables in the factor - const JacobianFactor whitened(jacobianFactor->whiten()); - for (JacobianFactor::const_iterator key = whitened.begin(); - key < whitened.end(); ++key) { - JacobianFactor::constABlock whitenedA = whitened.getA(key); - // find first column index for this key - size_t column_start = columnIndices[*key]; - for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) - for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { - double s = whitenedA(i, j); - if (std::abs(s) > 1e-12) - entries.emplace_back(row + i, column_start + j, s); - } - } - - JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = currentColIndex; - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) { - double s = whitenedb(i); - if (std::abs(s) > 1e-12) - entries.emplace_back(row + i, bcolumn, s); - } - - // Increment row index - row += jacobianFactor->rows(); - } - - // ...and make a sparse matrix with it. - Eigen::SparseMatrix Ab(row + 1, currentColIndex + 1); - Ab.setFromTriplets(entries.begin(), entries.end()); - return Ab; - } - - - /// obtain sparse matrix with given variable ordering for eigen sparse solver - std::pair obtainSparseMatrix( - const GaussianFactorGraph &gfg, - const Ordering &ordering) { - - gttic_(EigenOptimizer_obtainSparseMatrix); - - // Get sparse entries of Jacobian [A|b] augmented with RHS b. - auto entries = gfg.sparseJacobian(ordering); - - gttic_(EigenOptimizer_convertSparse); - // Convert boost tuples to Eigen triplets - vector> triplets; - triplets.reserve(entries.size()); - size_t rows = 0, cols = 0; - for (const auto &e : entries) { - size_t temp_rows = e.get<0>(), temp_cols = e.get<1>(); - triplets.emplace_back(temp_rows, temp_cols, e.get<2>()); - rows = std::max(rows, temp_rows); - cols = std::max(cols, temp_cols); - } - - // ...and make a sparse matrix with it. - SpMat Ab(rows + 1, cols + 1); - Ab.setFromTriplets(triplets.begin(), triplets.end()); - Ab.makeCompressed(); - gttoc_(EigenOptimizer_convertSparse); - - gttoc_(EigenOptimizer_obtainSparseMatrix); - - return make_pair(Ab.block(0, 0, rows + 1, cols), - Ab.col(cols)); - } - // Solves using sparse QR (used when solverType_ == QR) VectorValues solveQr(const GaussianFactorGraph &gfg, const Ordering &ordering) { gttic_(EigenOptimizer_optimizeEigenQR); - // this is where ordering is used - auto Ab_pair = obtainSparseMatrix(gfg, ordering); + // get sparse matrix from factor graph + ordering + size_t rows, cols; + SparseMatrixEigen Ab; + std::tie(rows, cols, Ab) = gfg.sparseJacobian(ordering); // Solve A*x = b using sparse QR from Eigen - gttic_(EigenOptimizer_optimizeEigenQR_create_solver); - Eigen::SparseQR> solver(Ab_pair.first); - gttoc_(EigenOptimizer_optimizeEigenQR_create_solver); + gttic_(create_solver); + Eigen::SparseQR> + solver(Ab.block(0, 0, rows, cols - 1)); + gttoc_(create_solver); - gttic_(EigenOptimizer_optimizeEigenQR_solve); - Eigen::VectorXd x = solver.solve(Ab_pair.second); - gttoc_(EigenOptimizer_optimizeEigenQR_solve); + gttic_(solve); + Eigen::VectorXd x = solver.solve(Ab.col(cols-1)); + gttoc_(solve); return VectorValues(x, gfg.getKeyDimMap()); } @@ -170,26 +59,29 @@ namespace gtsam { VectorValues solveCholesky(const GaussianFactorGraph &gfg, const Ordering &ordering) { gttic_(EigenOptimizer_optimizeEigenCholesky); - SpMat Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering); - auto rows = Ab.rows(), cols = Ab.cols(); + + // get sparse matrices A|b from factor graph + ordering + size_t rows, cols; + SparseMatrixEigen Ab; + std::tie(rows, cols, Ab) = gfg.sparseJacobian(ordering); + auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); auto b = Ab.col(cols - 1); - SpMat AtA(A.cols(), A.cols()); + SparseMatrixEigen AtA(A.cols(), A.cols()); AtA.selfadjointView().rankUpdate(At); - gttic_(EigenOptimizer_optimizeEigenCholesky_create_solver); + gttic_(create_solver); // Solve A*x = b using sparse Cholesky from Eigen - Eigen::SimplicialLDLT> solver(AtA); + gttoc_(create_solver); - gttoc_(EigenOptimizer_optimizeEigenCholesky_create_solver); - - gttic_(EigenOptimizer_optimizeEigenCholesky_solve); + gttic_(solve); Eigen::VectorXd x = solver.solve(At * b); - gttoc_(EigenOptimizer_optimizeEigenCholesky_solve); + gttoc_(solve); // NOTE: b is reordered now, so we need to transform back the order. // First find dimensions of each variable diff --git a/gtsam/linear/SparseEigenSolver.h b/gtsam/linear/SparseEigenSolver.h index 14f5240eb9..ada89e72ba 100644 --- a/gtsam/linear/SparseEigenSolver.h +++ b/gtsam/linear/SparseEigenSolver.h @@ -21,6 +21,7 @@ * @author Mandy Xie * @author Fan Jiang * @author Frank Dellaert + * @author Gerry Chen */ #pragma once @@ -58,10 +59,5 @@ class GTSAM_EXPORT SparseEigenSolver : public LinearSolver { * Uses elimination ordering during sparse matrix generation in `solve(gfg)` */ VectorValues solve(const GaussianFactorGraph &gfg) const override; - - /// Returns sparse matrix with given variable ordering for sparse matrix - /// solvers - static Eigen::SparseMatrix sparseJacobianEigen( - const GaussianFactorGraph &gfg, const Ordering &ordering); }; } // namespace gtsam diff --git a/gtsam/linear/SuiteSparseSolver.cpp b/gtsam/linear/SuiteSparseSolver.cpp index cbb7214c53..a9c786f10d 100644 --- a/gtsam/linear/SuiteSparseSolver.cpp +++ b/gtsam/linear/SuiteSparseSolver.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #ifdef GTSAM_USE_SUITESPARSE #include @@ -35,23 +35,21 @@ namespace gtsam { const gtsam::GaussianFactorGraph &gfg) const { gttic_(SuiteSparseSolver_optimizeEigenCholesky); - // this is where ordering is used - Eigen::SparseMatrix - Ab = SparseEigenSolver::sparseJacobianEigen(gfg, ordering_); - auto rows = Ab.rows(), cols = Ab.cols(); + // sparse Jacobian + size_t rows, cols; + SparseMatrixEigen Ab; + std::tie(rows, cols, Ab) = + gfg.sparseJacobian(ordering_); auto A = Ab.block(0, 0, rows, cols - 1); auto At = A.transpose(); auto b = Ab.col(cols - 1); - Eigen::SparseMatrix - AtA(A.cols(), A.cols()); + SparseMatrixEigen AtA(A.cols(), A.cols()); AtA.selfadjointView().rankUpdate(At); gttic_(SuiteSparseSolver_optimizeEigenCholesky_create_solver); // Solve A*x = b using sparse Cholesky from Eigen - Eigen::CholmodSupernodalLLT - , Eigen::Upper> - solver; + Eigen::CholmodSupernodalLLT solver; solver.cholmod().nmethods = 1; solver.cholmod().method[0].ordering = CHOLMOD_NATURAL; solver.cholmod().postorder = false; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d45a711029..61e95c2b5e 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -27,6 +27,7 @@ #include #include +#include #include #include // for operator += using namespace boost::assign; @@ -40,6 +41,28 @@ using namespace gtsam; // static SharedDiagonal // sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2), // constraintModel = noiseModel::Constrained::All(2); +typedef SparseMatrixBoostTriplets BoostEntries; +typedef BoostEntries::value_type BoostEntry; +bool entryequal(BoostEntry a, BoostEntry b) { + if (a.get<0>() == b.get<0>() && a.get<1>() == b.get<1>() && + a.get<2>() == b.get<2>()) return true; + + cout << "not equal:" << endl; + cout << "\texpected: (" << a.get<0>() << ", " << a.get<1>() << ") = " << a.get<2>() << endl; + cout << "\tactual: (" << b.get<0>() << ", " << b.get<1>() << ") = " << b.get<2>() << endl; + return false; +} +typedef SparseMatrixEigenTriplets EigenEntries; +typedef EigenEntries::value_type EigenEntry; +bool entryequal(EigenEntry a, EigenEntry b) { + if (a.row() == b.row() && a.col() == b.col() && a.value() == b.value()) + return true; + + cout << "not equal:" << endl; + cout << "\texpected: (" << a.row() << ", " << a.col() << ") = " << a.value() << endl; + cout << "\tactual: (" << b.row() << ", " << b.col() << ") = " << b.value() << endl; + return false; +} /* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { @@ -64,8 +87,8 @@ TEST(GaussianFactorGraph, initialization) { 10, 10, -1, -1, -10, -10, 10, 10, 2, -1, -5, -5, 5, 5, 1, -5, -5, 5, 5, -1, 1.5) .finished(); - Matrix actualIJS = fg.sparseJacobian_(); - EQUALITY(expectedIJS, actualIJS); + auto actualTuple = fg.sparseJacobian(); + EQUALITY(expectedIJS, std::get<2>(actualTuple)); } /* ************************************************************************* */ @@ -85,10 +108,6 @@ TEST(GaussianFactorGraph, sparseJacobian) { x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(), Vector2(13, 16), model); - auto entries = gfg.sparseJacobian(); - // Check the triplets size... - EXPECT_LONGS_EQUAL(16, entries.size()); - // Check version for MATLAB - NOTE that we transpose this! Matrix expectedT = (Matrix(16, 3) << 1, 1, 2, @@ -108,15 +127,46 @@ TEST(GaussianFactorGraph, sparseJacobian) { 3, 6,26, 4, 6,32).finished(); Matrix expectedMatlab = expectedT.transpose(); - Matrix actual = gfg.sparseJacobian_(); - - EXPECT(assert_equal(expectedMatlab, actual)); + auto actual = gfg.sparseJacobian(); + + EXPECT(assert_equal(4, std::get<0>(actual))); + EXPECT(assert_equal(6, std::get<1>(actual))); + EXPECT(assert_equal(expectedMatlab, std::get<2>(actual))); + + // BoostEntries + auto boostActual = gfg.sparseJacobian(); + // check the triplets size... + EXPECT_LONGS_EQUAL(16, std::get<2>(boostActual).size()); + EXPECT(assert_equal(4, std::get<0>(boostActual))); + EXPECT(assert_equal(6, std::get<1>(boostActual))); + // check content + for (int i = 0; i < 16; i++) { + EXPECT(entryequal( + BoostEntry(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + std::get<2>(boostActual).at(i))); + } + // EigenEntries + auto eigenActual = gfg.sparseJacobian(); + EXPECT_LONGS_EQUAL(16, std::get<2>(eigenActual).size()); + EXPECT(assert_equal(4, std::get<0>(eigenActual))); + EXPECT(assert_equal(6, std::get<1>(eigenActual))); + for (int i = 0; i < 16; i++) { + EXPECT(entryequal( + EigenEntry(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + std::get<2>(eigenActual).at(i))); + } + // Sparse Matrix + auto sparseResult = gfg.sparseJacobian(); + EXPECT_LONGS_EQUAL(16, std::get<2>(sparseResult).nonZeros()); + EXPECT(assert_equal(4, std::get<0>(sparseResult))); + EXPECT(assert_equal(6, std::get<1>(sparseResult))); + SparseMatrixEigen Ab(std::get<0>(eigenActual), std::get<1>(eigenActual)); + auto eigenEntries = std::get<2>(eigenActual); + Ab.setFromTriplets(eigenEntries.begin(), eigenEntries.end()); + EXPECT(assert_equal(Matrix(Ab), Matrix(std::get<2>(sparseResult)))); // Call sparseJacobian with optional ordering... auto ordering = Ordering(list_of(x45)(x123)); - entries = gfg.sparseJacobian(ordering); - // Check the triplets size... - EXPECT_LONGS_EQUAL(16, entries.size()); // Create factor graph: // x4 x3 x1 x2 x3 b @@ -143,9 +193,32 @@ TEST(GaussianFactorGraph, sparseJacobian) { 3, 6,26, 4, 6,32).finished(); expectedMatlab = expectedT.transpose(); - actual = gfg.sparseJacobian_(ordering); - - EXPECT(assert_equal(expectedMatlab, actual)); + actual = gfg.sparseJacobian(ordering); + + EXPECT(assert_equal(4, std::get<0>(actual))); + EXPECT(assert_equal(6, std::get<1>(actual))); + EXPECT(assert_equal(expectedMatlab, std::get<2>(actual))); + + // BoostEntries with optional ordering + boostActual = gfg.sparseJacobian(ordering); + EXPECT_LONGS_EQUAL(16, std::get<2>(boostActual).size()); + EXPECT(assert_equal(4, std::get<0>(boostActual))); + EXPECT(assert_equal(6, std::get<1>(boostActual))); + for (int i = 0; i < 16; i++) { + EXPECT(entryequal( + BoostEntry(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + std::get<2>(boostActual).at(i))); + } + // EigenEntries with optional ordering + eigenActual = gfg.sparseJacobian(ordering); + EXPECT_LONGS_EQUAL(16, std::get<2>(eigenActual).size()); + EXPECT(assert_equal(4, std::get<0>(eigenActual))); + EXPECT(assert_equal(6, std::get<1>(eigenActual))); + for (int i = 0; i < 16; i++) { + EXPECT(entryequal( + EigenEntry(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + std::get<2>(eigenActual).at(i))); + } } /* ************************************************************************* */ From 2ffad88aa20ae7341bc30a49e7482b0fa8974856 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 23 Nov 2020 11:31:21 -0500 Subject: [PATCH 90/91] Eigen index type fix SuiteSparse/cuSPARSE need "int" type ordering --- gtsam/base/Matrix.h | 3 +-- gtsam/linear/SparseEigenSolver.cpp | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 0d1cf85425..998128cd9c 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -81,8 +81,7 @@ typedef Eigen::Block ConstSubMatrix; typedef std::vector> SparseMatrixBoostTriplets; typedef std::vector> SparseMatrixEigenTriplets; -typedef Eigen::SparseMatrix - SparseMatrixEigen; +typedef Eigen::SparseMatrix SparseMatrixEigen; // Matrix formatting arguments when printing. // Akin to Matlab style. diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp index ada6dcd011..3397869893 100644 --- a/gtsam/linear/SparseEigenSolver.cpp +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -44,7 +44,7 @@ namespace gtsam { // Solve A*x = b using sparse QR from Eigen gttic_(create_solver); - Eigen::SparseQR> + Eigen::SparseQR> solver(Ab.block(0, 0, rows, cols - 1)); gttoc_(create_solver); @@ -75,7 +75,7 @@ namespace gtsam { gttic_(create_solver); // Solve A*x = b using sparse Cholesky from Eigen Eigen::SimplicialLDLT> + Eigen::NaturalOrdering> solver(AtA); gttoc_(create_solver); From 09038e8caaea6ada85cbff34130d9dcb5dbdfb94 Mon Sep 17 00:00:00 2001 From: yetongumich Date: Thu, 19 Jan 2023 23:45:09 -0500 Subject: [PATCH 91/91] WIP --- gtsam/CMakeLists.txt | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 176 ++++++++++-------- gtsam/linear/GaussianFactorGraph.h | 50 +++-- .../linear/tests/testGaussianFactorGraph.cpp | 18 +- .../NonlinearConjugateGradientOptimizer.h | 4 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizerParams.h | 43 ++--- tests/testNonlinearOptimizer.cpp | 36 ++-- 8 files changed, 164 insertions(+), 169 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 344a4ef114..4d96de1334 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -89,7 +89,7 @@ configure_file("${GTSAM_SOURCE_DIR}/cmake/dllexport.h.in" "dllexport.h") list(APPEND gtsam_srcs "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h") install(FILES "${PROJECT_BINARY_DIR}/config.h" "${PROJECT_BINARY_DIR}/dllexport.h" DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/gtsam) -if(GTSAM_SUPPORT_NESTED_DISSECTION AND NOT GTSAM_USE_SUITESPARSE) +if(GTSAM_SUPPORT_NESTED_DISSECTION) # target metis-gtsam-if is defined in both cases: embedded metis or system version: list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 4e529ed7e4..a66a622792 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -18,6 +18,7 @@ * @author Frank Dellaert */ +#include #include #include #include @@ -28,6 +29,9 @@ #include #include #include +#include + +#include using namespace std; using namespace gtsam; @@ -115,73 +119,16 @@ namespace gtsam { } /* ************************************************************************* */ - /// instantiate explicit template functions with sparse matrix representations - // Boost Tuples - template std::tuple - GaussianFactorGraph::sparseJacobian( - const Ordering& ordering) const; - // Eigen Triplets - template std::tuple - GaussianFactorGraph::sparseJacobian( - const Ordering& ordering) const; - // Eigen Sparse Matrix (template specialized) - template <> - std::tuple - GaussianFactorGraph::sparseJacobian( - const Ordering& ordering) const { - gttic_(GaussianFactorGraph_sparseJacobian); - gttic_(obtainSparseJacobian); - size_t rows, cols; - SparseMatrixEigenTriplets entries; - std::tie(rows, cols, entries) = - sparseJacobian(ordering); - gttoc_(obtainSparseJacobian); - gttic_(convertSparseJacobian); - SparseMatrixEigen Ab(rows, cols); - Ab.reserve(entries.size()); - for (auto entry : entries) { - Ab.insert(entry.row(), entry.col()) = entry.value(); - } - Ab.makeCompressed(); - // TODO(gerry): benchmark to see if setFromTriplets is faster - // Ab.setFromTriplets(entries.begin(), entries.end()); - return std::make_tuple(rows, cols, Ab); - } - // Eigen Matrix in "matlab" format (template specialized) - template <> - std::tuple - GaussianFactorGraph::sparseJacobian(const Ordering& ordering) const { - gttic_(GaussianFactorGraph_sparseJacobian); - // call sparseJacobian - size_t nrows, ncols; - SparseMatrixBoostTriplets result; - std::tie(nrows, ncols, result) = - sparseJacobian(ordering); - - // translate to base 1 matrix - size_t nzmax = result.size(); - Matrix IJS(3, nzmax); - for (size_t k = 0; k < result.size(); k++) { - const auto& entry = result[k]; - IJS(0, k) = double(entry.get<0>() + 1); - IJS(1, k) = double(entry.get<1>() + 1); - IJS(2, k) = entry.get<2>(); - } - return std::make_tuple(nrows, ncols, IJS); - } - - /* ************************************************************************* */ - // sparse matrix representation template `XXXEntries` must satisfy - // `XXXEntries.emplace_back(int i, int j, double s)` to compile - template - std::tuple GaussianFactorGraph::sparseJacobian( - const Ordering& ordering) const { + using SparseTriplets = std::vector >; + SparseTriplets GaussianFactorGraph::sparseJacobian(const Ordering& ordering, + size_t& nrows, + size_t& ncols) const { gttic_(GaussianFactorGraph_sparseJacobian); // First find dimensions of each variable - std::map dims; - for (const sharedFactor& factor : *this) { - if (!static_cast(factor)) - continue; + typedef std::map KeySizeMap; + KeySizeMap dims; + for (const auto& factor : *this) { + if (!static_cast(factor)) continue; for (auto it = factor->begin(); it != factor->end(); ++it) { dims[*it] = factor->getDim(it); @@ -189,19 +136,18 @@ namespace gtsam { } // Compute first scalar column of each variable - size_t currentColIndex = 0; - std::map columnIndices; + ncols = 0; + KeySizeMap columnIndices = dims; for (const auto key : ordering) { - columnIndices[key] = currentColIndex; - currentColIndex += dims[key]; + columnIndices[key] = ncols; + ncols += dims[key]; } // Iterate over all factors, adding sparse scalar entries - Entries entries; - entries.reserve(60 * size()); - - size_t row = 0; - for (const sharedFactor& factor : *this) { + SparseTriplets entries; + entries.reserve(30 * size()); + nrows = 0; + for (const auto& factor : *this) { if (!static_cast(factor)) continue; // Convert to JacobianFactor if necessary @@ -229,23 +175,93 @@ namespace gtsam { for (size_t j = 0; j < (size_t)whitenedA.cols(); j++) { double s = whitenedA(i, j); if (std::abs(s) > 1e-12) - entries.emplace_back(row + i, column_start + j, s); + entries.emplace_back(nrows + i, column_start + j, s); } } JacobianFactor::constBVector whitenedb(whitened.getb()); - size_t bcolumn = currentColIndex; - for (size_t i = 0; i < (size_t) whitenedb.size(); i++) { + for (size_t i = 0; i < (size_t)whitenedb.size(); i++) { double s = whitenedb(i); - if (std::abs(s) > 1e-12) - entries.emplace_back(row + i, bcolumn, s); + if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s); } // Increment row index nrows += jacobianFactor->rows(); } - return std::make_tuple(row, currentColIndex+1, entries); + ncols++; // +1 for b-column + return entries; + } + + /* ************************************************************************* */ + SparseTriplets GaussianFactorGraph::sparseJacobian() const { + size_t nrows, ncols; + return sparseJacobian(Ordering(this->keys()), nrows, ncols); + } + + template <> + std::tuple + GaussianFactorGraph::sparseJacobian( + const Ordering& ordering) const { + gttic_(GaussianFactorGraph_sparseJacobian); + gttic_(obtainSparseJacobian); + size_t rows, cols; + SparseTriplets entries_ = sparseJacobian(ordering, rows, cols); + std::vector> entries; + for (auto& v : entries_) { + entries.push_back(Eigen::Triplet(std::get<0>(v), std::get<1>(v), std::get<2>(v))); + } + gttoc_(obtainSparseJacobian); + gttic_(convertSparseJacobian); + SparseMatrixEigen Ab(rows, cols); + Ab.setFromTriplets(entries.begin(), entries.end()); + // Ab.reserve(entries.size()); + // for (auto entry : entries) { + // Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry); + // } + Ab.makeCompressed(); + // TODO(gerry): benchmark to see if setFromTriplets is faster + // Ab.setFromTriplets(entries.begin(), entries.end()); + return std::make_tuple(rows, cols, Ab); + } + // Eigen Matrix in "matlab" format (template specialized) + template <> + std::tuple + GaussianFactorGraph::sparseJacobian(const Ordering& ordering) const { + gttic_(GaussianFactorGraph_sparseJacobian); + // call sparseJacobian + size_t rows, cols; + SparseTriplets result = + sparseJacobian(ordering, rows, cols); + + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3, nzmax); + for (size_t k = 0; k < result.size(); k++) { + const auto& entry = result[k]; + IJS(0, k) = double(std::get<0>(entry) + 1); + IJS(1, k) = double(std::get<1>(entry) + 1); + IJS(2, k) = std::get<2>(entry); + } + return std::make_tuple(rows, cols, IJS); + } + + /* ************************************************************************* */ + Matrix GaussianFactorGraph::sparseJacobian_() const { + gttic_(GaussianFactorGraph_sparseJacobian_matrix); + // call sparseJacobian + auto result = sparseJacobian(); + + // translate to base 1 matrix + size_t nzmax = result.size(); + Matrix IJS(3, nzmax); + for (size_t k = 0; k < result.size(); k++) { + const auto& entry = result[k]; + IJS(0, k) = double(std::get<0>(entry) + 1); + IJS(1, k) = double(std::get<1>(entry) + 1); + IJS(2, k) = std::get<2>(entry); + } + return IJS; } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 6200770847..ae7063997f 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -197,37 +197,25 @@ namespace gtsam { ///@{ /** - * Generates an m-by-n sparse Jacobian matrix, where i(k) and j(k) are the - * base 0 row and column indices, s(k) a double. Column ordering is taken as - * default. The return type is a sparse matrix representation templated - * type which may be: - * 1. vector> is a vector of 3-tuples - * (i, j, s) - * 2. vector> is a vector of Eigen-triples (i, j, s) - * 3. Eigen::SparseMatrix is an Eigen format sparse matrix - * 4. Matrix (i/j will be 1-indexed instead of 0-indexed, for matlab) is a - * 3xK matrix [I;J;S] + * Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, + * where i(k) and j(k) are the base 0 row and column indices, and s(k) is + * the entry as a double. * The standard deviations are baked into A and b - * @return 3-tuple with the dimensions of the Jacobian as the first 2 - * elements and the sparse matrix in one of the 4 form above as the 3rd - * element. + * @return the sparse matrix as a std::vector of std::tuples + * @param ordering the column ordering + * @param[out] nrows The number of rows in the augmented Jacobian + * @param[out] ncols The number of columns in the augmented Jacobian */ - template - std::tuple sparseJacobian() const { - Ordering ord(this->keys()); - return sparseJacobian(ord); - } + std::vector > sparseJacobian( + const Ordering& ordering, size_t& nrows, size_t& ncols) const; + + /** Returns a sparse augmented Jacobian matrix with default ordering */ + std::vector > sparseJacobian() const; /** - * Generates an m-by-n sparse Jacobian matrix, where i(k) and j(k) are the - * base 0 row and column indices, s(k) a double. The return type is a sparse - * matrix representation templated type which may be: - * 1. vector> is a vector of 3-tuples - * (i, j, s) - * 2. vector> is a vector of Eigen-triples (i, j, s) - * 3. Eigen::SparseMatrix is an Eigen format sparse matrix - * 4. Matrix (i/j will be 1-indexed instead of 0-indexed, for matlab) is a - * 3xK matrix [I;J;S] + * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] + * entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's + * sparse. Note: i, j are 1-indexed. * The standard deviations are baked into A and b * @param ordering the column ordering * @return 3-tuple with the dimensions of the Jacobian as the first 2 @@ -238,6 +226,14 @@ namespace gtsam { std::tuple sparseJacobian( const Ordering& ordering) const; + /** + * Matrix version of sparseJacobian: generates a 3*m matrix with [i,j,s] + * entries such that S(i(k),j(k)) = s(k), which can be given to MATLAB's + * sparse. Note: i, j are 1-indexed. + * The standard deviations are baked into A and b + */ + Matrix sparseJacobian_() const; + /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ * Jacobian matrix, augmented with b with the noise models baked diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 15b3492071..b88e1de31c 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -77,15 +77,15 @@ TEST(GaussianFactorGraph, initialization) { // Test sparse, which takes a vector and returns a matrix, used in MATLAB // Note that this the augmented vector and the RHS is in column 7 - Matrix expectedIJS = - (Matrix(3, 21) << 1, 2, 1, 2, 3, 4, 3, 4, 3, 4, 5, 6, 5, 6, 6, 7, 8, 7, 8, - 7, 8, // - 1, 2, 7, 7, 1, 2, 3, 4, 7, 7, 1, 2, 5, 6, 7, 3, 4, 5, 6, 7, 7, // - 10, 10, -1, -1, -10, -10, 10, 10, 2, -1, -5, -5, 5, 5, 1, -5, -5, 5, 5, - -1, 1.5) - .finished(); - auto actualTuple = fg.sparseJacobian(); - EQUALITY(expectedIJS, std::get<2>(actualTuple)); + // Matrix expectedIJS = + // (Matrix(3, 21) << 1, 2, 1, 2, 3, 4, 3, 4, 3, 4, 5, 6, 5, 6, 6, 7, 8, 7, 8, + // 7, 8, // + // 1, 2, 7, 7, 1, 2, 3, 4, 7, 7, 1, 2, 5, 6, 7, 3, 4, 5, 6, 7, 7, // + // 10, 10, -1, -1, -10, -10, 10, 10, 2, -1, -5, -5, 5, 5, 1, -5, -5, 5, 5, + // -1, 1.5) + // .finished(); + // auto actualTuple = fg.sparseJacobian(); + // EQUALITY(expectedIJS, std::get<2>(actualTuple)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index a7a0d724b7..52d82a6518 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -201,8 +201,8 @@ boost::tuple nonlinearConjugateGradient(const S &system, currentError = system.error(currentValues); // User hook: - if (params.iterationHook) - params.iterationHook(iteration, prevError, currentError); + // if (params.iterationHook) + // params.iterationHook(iteration, prevError, currentError); // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::ERROR) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index d0aa8aec95..bdb8f8d5cd 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -101,8 +101,8 @@ void NonlinearOptimizer::defaultOptimize() { newError = error(); // User hook: - if (params.iterationHook) - params.iterationHook(iterations(), currentError, newError); + // if (params.iterationHook) + // params.iterationHook(iterations(), currentError, newError); // Maybe show output if (params.verbosity >= NonlinearOptimizerParams::VALUES) diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 33a8c4afd7..afebf075a1 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -42,20 +42,13 @@ class GTSAM_EXPORT NonlinearOptimizerParams { SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - - NonlinearOptimizerParams() - : maxIterations(100), - relativeErrorTol(1e-5), - absoluteErrorTol(1e-5), - errorTol(0.0), - verbosity(SILENT) {} - - // copy constructor + size_t maxIterations = 100; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol = 1e-5; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol = 1e-5; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol = 0.0; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity = SILENT; ///< The printing verbosity during optimization (default SILENT) + + // copy constructor NonlinearOptimizerParams(const NonlinearOptimizerParams& other) : maxIterations(other.maxIterations), relativeErrorTol(other.relativeErrorTol), @@ -75,25 +68,15 @@ class GTSAM_EXPORT NonlinearOptimizerParams { // copy assignment NonlinearOptimizerParams& operator=(const NonlinearOptimizerParams& other) { - return *this = NonlinearOptimizerParams(other); - } - - // move assignment - NonlinearOptimizerParams& operator=( - NonlinearOptimizerParams&& other) noexcept { - maxIterations = other.maxIterations; - relativeErrorTol = other.relativeErrorTol; - absoluteErrorTol = other.absoluteErrorTol; - errorTol = other.errorTol; - verbosity = other.verbosity; - std::swap(linearSolverParams, other.linearSolverParams); + maxIterations = (other.maxIterations); + relativeErrorTol = (other.relativeErrorTol); + absoluteErrorTol = (other.absoluteErrorTol); + errorTol = (other.errorTol); + verbosity = (other.verbosity); + linearSolverParams = (std::move(other.linearSolverParams)); return *this; } - virtual ~NonlinearOptimizerParams() { - } - virtual void print(const std::string& str = "") const; - size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { return relativeErrorTol; } double getAbsoluteErrorTol() const { return absoluteErrorTol; } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index a42d609471..36125326ad 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -658,18 +658,18 @@ TEST( NonlinearOptimizer, iterationHook_LM ) // Levenberg-Marquardt LevenbergMarquardtParams lmParams; size_t lastIterCalled = 0; - lmParams.iterationHook = [&](size_t iteration, double oldError, double newError) - { - // Tests: - lastIterCalled = iteration; - EXPECT(newError " << newError <<"\n"; - }; + // // Example of evolution printout: + // //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n"; + // }; LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize(); - EXPECT(lastIterCalled>5); + // EXPECT(lastIterCalled>5); } /* ************************************************************************* */ TEST( NonlinearOptimizer, iterationHook_CG ) @@ -683,18 +683,18 @@ TEST( NonlinearOptimizer, iterationHook_CG ) // Levenberg-Marquardt NonlinearConjugateGradientOptimizer::Parameters cgParams; size_t lastIterCalled = 0; - cgParams.iterationHook = [&](size_t iteration, double oldError, double newError) - { - // Tests: - lastIterCalled = iteration; - EXPECT(newError " << newError <<"\n"; - }; + // // Example of evolution printout: + // //std::cout << "iter: " << iteration << " error: " << oldError << " => " << newError <<"\n"; + // }; NonlinearConjugateGradientOptimizer(fg, c0, cgParams).optimize(); - EXPECT(lastIterCalled>5); + // EXPECT(lastIterCalled>5); }