From 56e9b3ac9ffa2edac339ce54af4cafeac0467b26 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 18 Jan 2021 21:10:52 -0500 Subject: [PATCH 1/9] Mandy+Fan's original code for converting sparse matrices to Eigen format --- gtsam/linear/SparseEigenSolver.cpp | 231 +++++++++++++++++++++++++++++ gtsam/linear/SparseEigenSolver.h | 62 ++++++++ 2 files changed, 293 insertions(+) create mode 100644 gtsam/linear/SparseEigenSolver.cpp create mode 100644 gtsam/linear/SparseEigenSolver.h diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp new file mode 100644 index 0000000000..f0dfd83f31 --- /dev/null +++ b/gtsam/linear/SparseEigenSolver.cpp @@ -0,0 +1,231 @@ +/* ---------------------------------------------------------------------------- + + * 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 SparseEigenSolver.cpp + * + * @brief Eigen SparseSolver based linear solver backend for GTSAM + * + * @date Aug 2019 + * @author Mandy Xie + * @author Fan Jiang + * @author Frank Dellaert + */ + +#include +#include +#include + +#include + +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 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)); + } + + bool SparseEigenSolver::isIterative() { + return false; + } + + bool SparseEigenSolver::isSequential() { + return false; + } + + VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) { + 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); + 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); + } + } + + 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; + } + + throw std::exception(); + } + + 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 new file mode 100644 index 0000000000..d71365864e --- /dev/null +++ b/gtsam/linear/SparseEigenSolver.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * 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 SparseEigenSolver.h + * + * @brief Eigen SparseSolver based linear solver backend for GTSAM + * + * @date Aug 2019 + * @author Mandy Xie + * @author Fan Jiang + * @author Frank Dellaert + */ + +#pragma once + +#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; + + bool isSequential() override; + + VectorValues solve(const GaussianFactorGraph &gfg) override; + + static Eigen::SparseMatrix + sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering); + + protected: + + SparseEigenSolverType solverType = QR; + + Ordering ordering; + }; +} // namespace gtsam From a477ec681170ee7f5c4d9eadac460b595eeee972 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 18 Jan 2021 20:20:05 -0500 Subject: [PATCH 2/9] merge Mandy + Fan's sparseJacobian unit test additions --- .../linear/tests/testGaussianFactorGraph.cpp | 84 ++++++++++++------- 1 file changed, 52 insertions(+), 32 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 45f652d056..57a663e8c8 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -36,9 +36,18 @@ using namespace boost::assign; using namespace std; 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 boost::tuple BoostTriplet; +bool triplet_equal(BoostTriplet a, BoostTriplet 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; +} /* ************************************************************************* */ TEST(GaussianFactorGraph, initialization) { @@ -73,37 +82,48 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 - - // Expected - 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(); - - 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)); + 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); + + // 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(); + + // matrix form (matlab) + Matrix expectedMatlab = expectedT.transpose(); + EXPECT(assert_equal(expectedMatlab, gfg.sparseJacobian_())); + + // BoostTriplets + auto boostActual = gfg.sparseJacobian(); + // check the triplets size... + EXPECT_LONGS_EQUAL(16, boostActual.size()); + // check content + for (int i = 0; i < 16; i++) { + EXPECT(triplet_equal( + BoostTriplet(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + boostActual.at(i))); + } } /* ************************************************************************* */ From 44c232a128c4fac074079f11d91db0601edff127 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 18 Jan 2021 20:39:34 -0500 Subject: [PATCH 3/9] organize/isolate sparseEigen functionality --- gtsam/linear/SparseEigen.h | 146 ++++++++++++++++ gtsam/linear/SparseEigenSolver.cpp | 231 ------------------------- gtsam/linear/SparseEigenSolver.h | 62 ------- gtsam/linear/tests/testSparseEigen.cpp | 72 ++++++++ 4 files changed, 218 insertions(+), 293 deletions(-) create mode 100644 gtsam/linear/SparseEigen.h delete mode 100644 gtsam/linear/SparseEigenSolver.cpp delete mode 100644 gtsam/linear/SparseEigenSolver.h create mode 100644 gtsam/linear/tests/testSparseEigen.cpp diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h new file mode 100644 index 0000000000..a157be2b15 --- /dev/null +++ b/gtsam/linear/SparseEigen.h @@ -0,0 +1,146 @@ +/* ---------------------------------------------------------------------------- + + * 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 SparseEigen.h + * + * @brief Utilities for converting to Eigen's sparse matrix representations + * + * @date Aug 2019 + * @author Mandy Xie + * @author Fan Jiang + * @author Gerry Chen + * @author Frank Dellaert + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +typedef Eigen::SparseMatrix SpMat; + +SpMat 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 + std::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 std::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. + SpMat Ab(row + 1, currentColIndex + 1); + Ab.setFromTriplets(entries.begin(), entries.end()); + return Ab; +} + +SpMat sparseJacobianEigen(const GaussianFactorGraph &gfg) { + return sparseJacobianEigen(gfg, Ordering(gfg.keys())); +} + +// /// obtain sparse matrix 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)); +// } + +} // namespace gtsam diff --git a/gtsam/linear/SparseEigenSolver.cpp b/gtsam/linear/SparseEigenSolver.cpp deleted file mode 100644 index f0dfd83f31..0000000000 --- a/gtsam/linear/SparseEigenSolver.cpp +++ /dev/null @@ -1,231 +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 SparseEigenSolver.cpp - * - * @brief Eigen SparseSolver based linear solver backend for GTSAM - * - * @date Aug 2019 - * @author Mandy Xie - * @author Fan Jiang - * @author Frank Dellaert - */ - -#include -#include -#include - -#include - -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 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)); - } - - bool SparseEigenSolver::isIterative() { - return false; - } - - bool SparseEigenSolver::isSequential() { - return false; - } - - VectorValues SparseEigenSolver::solve(const GaussianFactorGraph &gfg) { - 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); - 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); - } - } - - 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; - } - - throw std::exception(); - } - - 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 deleted file mode 100644 index d71365864e..0000000000 --- a/gtsam/linear/SparseEigenSolver.h +++ /dev/null @@ -1,62 +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 SparseEigenSolver.h - * - * @brief Eigen SparseSolver based linear solver backend for GTSAM - * - * @date Aug 2019 - * @author Mandy Xie - * @author Fan Jiang - * @author Frank Dellaert - */ - -#pragma once - -#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; - - bool isSequential() override; - - VectorValues solve(const GaussianFactorGraph &gfg) override; - - static Eigen::SparseMatrix - sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering); - - protected: - - SparseEigenSolverType solverType = QR; - - Ordering ordering; - }; -} // namespace gtsam diff --git a/gtsam/linear/tests/testSparseEigen.cpp b/gtsam/linear/tests/testSparseEigen.cpp new file mode 100644 index 0000000000..225e1dab22 --- /dev/null +++ b/gtsam/linear/tests/testSparseEigen.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSparseMatrix.cpp + * @author Mandy Xie + * @author Fan Jiang + * @author Gerry Chen + * @author Frank Dellaert + * @date Jan, 2021 + */ + +#include +#include + +#include +using boost::assign::list_of; + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(SparseEigen, sparseJacobianEigen) { + GaussianFactorGraph gfg; + SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); + 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); + + // Sparse Matrix + auto sparseResult = sparseJacobianEigen(gfg); + EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros()); + EXPECT(assert_equal(4, sparseResult.rows())); + EXPECT(assert_equal(6, sparseResult.cols())); + EXPECT(assert_equal(gfg.augmentedJacobian(), Matrix(sparseResult))); + + // Call sparseJacobian with optional ordering... + auto ordering = Ordering(list_of(x45)(x123)); + + // Eigen Sparse with optional ordering + EXPECT(assert_equal(gfg.augmentedJacobian(ordering), + Matrix(sparseJacobianEigen(gfg, ordering)))); + + // Check matrix dimensions when zero rows / cols + gfg.add(x123, Matrix23::Zero(), Vector2::Zero(), model); // zero row + gfg.add(2, Matrix21::Zero(), Vector2::Zero(), model); // zero col + sparseResult = sparseJacobianEigen(gfg); + EXPECT_LONGS_EQUAL(16, sparseResult.nonZeros()); + EXPECT(assert_equal(8, sparseResult.rows())); + EXPECT(assert_equal(7, sparseResult.cols())); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From ee5701dcda6a3aeb05d30991bfd7aefd3bf7a443 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 18 Jan 2021 20:39:53 -0500 Subject: [PATCH 4/9] fix off-by-one bug --- gtsam/linear/SparseEigen.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index a157be2b15..35062c4fe4 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -102,7 +102,7 @@ SpMat sparseJacobianEigen( } // ...and make a sparse matrix with it. - SpMat Ab(row + 1, currentColIndex + 1); + SpMat Ab(row, currentColIndex + 1); Ab.setFromTriplets(entries.begin(), entries.end()); return Ab; } From d9c03aa827f3b8e30180c41dd94249515ad376c0 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 18 Jan 2021 20:56:44 -0500 Subject: [PATCH 5/9] cleanup --- gtsam/linear/SparseEigen.h | 37 +++++-------------------------------- 1 file changed, 5 insertions(+), 32 deletions(-) diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index 35062c4fe4..5ee67f6d94 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -32,8 +32,13 @@ namespace gtsam { typedef Eigen::SparseMatrix SpMat; +/// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph SpMat sparseJacobianEigen( const GaussianFactorGraph &gfg, const Ordering &ordering) { + // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version + // more general, or by creating an Eigen::Triplet compatible wrapper for + // boost::tuple return type + // First find dimensions of each variable std::map dims; for (const boost::shared_ptr &factor : gfg) { @@ -111,36 +116,4 @@ SpMat sparseJacobianEigen(const GaussianFactorGraph &gfg) { return sparseJacobianEigen(gfg, Ordering(gfg.keys())); } -// /// obtain sparse matrix 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)); -// } - } // namespace gtsam From d8491b27fb010e5d0061c8926730b11512433578 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jan 2021 10:53:22 -0500 Subject: [PATCH 6/9] rename matrix type from `SpMat` to `SparseEigen` --- gtsam/linear/SparseEigen.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index 5ee67f6d94..ee2a263305 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -30,10 +30,10 @@ namespace gtsam { -typedef Eigen::SparseMatrix SpMat; +typedef Eigen::SparseMatrix SparseEigen; /// Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph -SpMat sparseJacobianEigen( +SparseEigen sparseJacobianEigen( const GaussianFactorGraph &gfg, const Ordering &ordering) { // TODO(gerry): eliminate copy/pasta by making GaussianFactorGraph version // more general, or by creating an Eigen::Triplet compatible wrapper for @@ -107,12 +107,12 @@ SpMat sparseJacobianEigen( } // ...and make a sparse matrix with it. - SpMat Ab(row, currentColIndex + 1); + SparseEigen Ab(row, currentColIndex + 1); Ab.setFromTriplets(entries.begin(), entries.end()); return Ab; } -SpMat sparseJacobianEigen(const GaussianFactorGraph &gfg) { +SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) { return sparseJacobianEigen(gfg, Ordering(gfg.keys())); } From 25e3b5609e64999c8a42b6416cf5c4634693663c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jan 2021 11:01:25 -0500 Subject: [PATCH 7/9] roll back some cosmetic changes to minimize the diff --- .../linear/tests/testGaussianFactorGraph.cpp | 50 ++++++++++--------- 1 file changed, 27 insertions(+), 23 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 57a663e8c8..0ae5008245 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -82,6 +82,29 @@ TEST(GaussianFactorGraph, sparseJacobian) { // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 + + // Expected + Matrix expected = (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(); + + // expected: in matlab format - NOTE the transpose!) + Matrix expectedMatlab = expected.transpose(); + GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5); const Key x123 = 0, x45 = 1; @@ -91,28 +114,9 @@ TEST(GaussianFactorGraph, sparseJacobian) { x45, (Matrix(2, 2) << 11, 12, 14, 15.).finished(), Vector2(13, 16), model); - // 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(); - - // matrix form (matlab) - Matrix expectedMatlab = expectedT.transpose(); - EXPECT(assert_equal(expectedMatlab, gfg.sparseJacobian_())); + Matrix actual = gfg.sparseJacobian_(); + + EXPECT(assert_equal(expected, actual)); // BoostTriplets auto boostActual = gfg.sparseJacobian(); @@ -121,7 +125,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { // check content for (int i = 0; i < 16; i++) { EXPECT(triplet_equal( - BoostTriplet(expectedT(i, 0) - 1, expectedT(i, 1) - 1, expectedT(i, 2)), + BoostTriplet(expected(i, 0) - 1, expected(i, 1) - 1, expected(i, 2)), boostActual.at(i))); } } From e2f5be4e4700e26758f100881375627ae0368c64 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jan 2021 11:04:26 -0500 Subject: [PATCH 8/9] SparseEigen docstring --- gtsam/linear/SparseEigen.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h index ee2a263305..7963d3ef58 100644 --- a/gtsam/linear/SparseEigen.h +++ b/gtsam/linear/SparseEigen.h @@ -12,7 +12,7 @@ /** * @file SparseEigen.h * - * @brief Utilities for converting to Eigen's sparse matrix representations + * @brief Utilities for creating Eigen sparse matrices (gtsam::SparseEigen) * * @date Aug 2019 * @author Mandy Xie From b76993b171a44c902fcc6cbb45794bc245808042 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 19 Jan 2021 11:47:44 -0500 Subject: [PATCH 9/9] typo: `expected` changed to `expectedMatlab` --- gtsam/linear/tests/testGaussianFactorGraph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 0ae5008245..8b9ce94a9b 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -116,7 +116,7 @@ TEST(GaussianFactorGraph, sparseJacobian) { Matrix actual = gfg.sparseJacobian_(); - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(expectedMatlab, actual)); // BoostTriplets auto boostActual = gfg.sparseJacobian();