diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h new file mode 100644 index 000000000..f6620f162 --- /dev/null +++ b/gtsam/sfm/PowerMethod.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 PowerMethod.h + * @date Sept 2020 + * @author Jing Wu + * @brief accelerated power method for fast eigenvalue and eigenvector computation + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gtsam { + /* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS + +/** This is a lightweight struct used in conjunction with Spectra to compute + * the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single + * nontrivial function, perform_op(x,y), that computes and returns the product + * y = (A + sigma*I) x */ +struct MatrixProdFunctor { + // Const reference to an externally-held matrix whose minimum-eigenvalue we + // want to compute + const Sparse &A_; + + // Spectral shift + double sigma_; + + const int m_n_; + const int m_nev_; + const int m_ncv_; + Vector ritz_val_; + Matrix ritz_vectors_; + + // Constructor + explicit MatrixProdFunctor(const Sparse &A, int nev, int ncv, + double sigma = 0) + : A_(A), + m_n_(A.rows()), + m_nev_(nev), + m_ncv_(ncv > m_n_ ? m_n_ : ncv), + sigma_(sigma) {} + + int rows() const { return A_.rows(); } + int cols() const { return A_.cols(); } + + // Matrix-vector multiplication operation + void perform_op(const double *x, double *y) const { + // Wrap the raw arrays as Eigen Vector types + Eigen::Map X(x, rows()); + Eigen::Map Y(y, rows()); + + // Do the multiplication using wrapped Eigen vectors + Y = A_ * X + sigma_ * X; + } + + long next_long_rand(long seed) { + const unsigned int m_a = 16807; + const unsigned long m_max = 2147483647L; + long m_rand = m_n_ ? (m_n_ & m_max) : 1; + unsigned long lo, hi; + + lo = m_a * (long)(seed & 0xFFFF); + hi = m_a * (long)((unsigned long)seed >> 16); + lo += (hi & 0x7FFF) << 16; + if (lo > m_max) { + lo &= m_max; + ++lo; + } + lo += hi >> 15; + if (lo > m_max) { + lo &= m_max; + ++lo; + } + return (long)lo; + } + + Vector random_vec(const int len) { + Vector res(len); + const unsigned long m_max = 2147483647L; + for (int i = 0; i < len; i++) { + long m_rand = next_long_rand(m_rand); + res[i] = double(m_rand) / double(m_max) - double(0.5); + } + return res; + } + + void init(Vector data) { + ritz_val_.resize(m_ncv_); + ritz_val_.setZero(); + ritz_vectors_.resize(m_ncv_, m_nev_); + ritz_vectors_.setZero(); + Eigen::Map v0(init_resid.data(), m_n_); + } + + void init() { + Vector init_resid = random_vec(m_n_); + init(init_resid); + } + + bool converged(double tol, const Vector x) { + double theta = x.transpose() * A_ * x; + Vector diff = A_ * x - theta * x; + double error = diff.lpNorm<1>(); + return error < tol; + } + + int num_converged(double tol) { + int converge = 0; + for (int i=0; i= m_nev_) break; + + nev_adj = nev_adjusted(nconv); + restart(nev_adj); + } + // Sorting results + sort_ritzpair(sort_rule); + + m_niter += i + 1; + m_info = (nconv >= m_nev_) ? SUCCESSFUL : NOT_CONVERGING; + + return std::min(m_nev_, nconv); + } + + Vector eigenvalues() { + return ritz_val_; + } + + Matrix eigenvectors() { + return ritz_vectors_; + } + +}; + +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index df2d72c28..c2e2a528c 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -470,38 +471,55 @@ Sparse ShonanAveraging::computeA(const Values &values) const { return Lambda - Q_; } -/* ************************************************************************* */ -/// MINIMUM EIGENVALUE COMPUTATIONS +// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization +static bool PowerMinimumEigenValue( + const Sparse &A, const Matrix &S, double *minEigenValue, + Vector *minEigenVector = 0, size_t *numIterations = 0, + size_t maxIterations = 1000, + double minEigenvalueNonnegativityTolerance = 10e-4, + Eigen::Index numLanczosVectors = 20) { -/** This is a lightweight struct used in conjunction with Spectra to compute - * the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single - * nontrivial function, perform_op(x,y), that computes and returns the product - * y = (A + sigma*I) x */ -struct MatrixProdFunctor { - // Const reference to an externally-held matrix whose minimum-eigenvalue we - // want to compute - const Sparse &A_; + // a. Compute dominant eigenpair of S using power method + MatrixProdFunctor lmOperator(A, 1, std::min(numLanczosVectors, A.rows())); + lmOperator.init(); - // Spectral shift - double sigma_; + const int lmConverged = lmEigenValueSolver.compute( + maxIterations, 1e-4); - // Constructor - explicit MatrixProdFunctor(const Sparse &A, double sigma = 0) - : A_(A), sigma_(sigma) {} + // Check convergence and bail out if necessary + if (lmConverged != 1) return false; - int rows() const { return A_.rows(); } - int cols() const { return A_.cols(); } + const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0); - // Matrix-vector multiplication operation - void perform_op(const double *x, double *y) const { - // Wrap the raw arrays as Eigen Vector types - Eigen::Map X(x, rows()); - Eigen::Map Y(y, rows()); - - // Do the multiplication using wrapped Eigen vectors - Y = A_ * X + sigma_ * X; + if (lmEigenValue < 0) { + // The largest-magnitude eigenvalue is negative, and therefore also the + // minimum eigenvalue, so just return this solution + *minEigenValue = lmEigenValue; + if (minEigenVector) { + *minEigenVector = lmEigenValueSolver.eigenvectors().col(0); + minEigenVector->normalize(); // Ensure that this is a unit vector + } + return true; } -}; + + Matrix C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + MatrixProdFunctor minShiftedOperator( + C, 1, std::min(numLanczosVectors, A.rows())); + minShiftedOperator.init(); + + const int minConverged = minShiftedOperator.compute( + maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); + + if (minConverged != 1) return false; + + *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues()(0); + if (minEigenVector) { + *minEigenVector = minShiftedOperator.eigenvectors().col(0); + minEigenVector->normalize(); // Ensure that this is a unit vector + } + if (numIterations) *numIterations = minShiftedOperator.num_iterations(); + return true; +} /// Function to compute the minimum eigenvalue of A using Lanczos in Spectra. /// This does 2 things: diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/sfm/tests/testPowerMethod.cpp new file mode 100644 index 000000000..f67fad458 --- /dev/null +++ b/gtsam/sfm/tests/testPowerMethod.cpp @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010-2019, 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 + + * -------------------------------------------------------------------------- */ + +/** + * testPowerMethod.cpp + * + * @file testPowerMethod.cpp + * @date Sept 2020 + * @author Jing Wu + * @brief Check eigenvalue and eigenvector computed by power method + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(PowerMethod, initialize) { + gtsam::Sparse A; + MatrixProdFunctor(A, 1, A.rows()); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */