diff --git a/gtsam/linear/SparseEigen.h b/gtsam/linear/SparseEigen.h new file mode 100644 index 000000000..a157be2b1 --- /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 f0dfd83f3..000000000 --- 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 d71365864..000000000 --- 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 000000000..225e1dab2 --- /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); +} +/* ************************************************************************* */