From 2afbaaaf441c6825f4a0c50a79db70c727e7d5d2 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sun, 13 Sep 2020 17:28:15 -0400 Subject: [PATCH 01/22] Add acc power method in Alg6 --- gtsam/sfm/PowerMethod.h | 166 ++++++++++++++++++++++++++++ gtsam/sfm/ShonanAveraging.cpp | 70 +++++++----- gtsam/sfm/tests/testPowerMethod.cpp | 38 +++++++ 3 files changed, 248 insertions(+), 26 deletions(-) create mode 100644 gtsam/sfm/PowerMethod.h create mode 100644 gtsam/sfm/tests/testPowerMethod.cpp 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); +} +/* ************************************************************************* */ From e02633c74582c0bfa3674af78691c506e780ab43 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 14 Sep 2020 01:58:44 -0400 Subject: [PATCH 02/22] Completed and tested power method with unittest --- gtsam/sfm/PowerMethod.h | 157 +++++++++++++++++++++------- gtsam/sfm/tests/testPowerMethod.cpp | 35 ++++++- 2 files changed, 149 insertions(+), 43 deletions(-) diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h index f6620f162..3b83fb8a4 100644 --- a/gtsam/sfm/PowerMethod.h +++ b/gtsam/sfm/PowerMethod.h @@ -24,14 +24,17 @@ #include #include -#include #include #include #include #include #include +#include namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + /* ************************************************************************* */ /// MINIMUM EIGENVALUE COMPUTATIONS @@ -39,46 +42,63 @@ namespace gtsam { * 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 { +struct PowerFunctor { // 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_; // dimension of Matrix A + const int m_nev_; // number of eigenvalues required - const int m_n_; - const int m_nev_; - const int m_ncv_; - Vector ritz_val_; - Matrix ritz_vectors_; + // a Polyak momentum term + double beta_; + // const int m_ncv_; // dimention of orthonormal basis subspace + size_t m_niter_; // number of iterations + +private: + Vector ritz_val_; // all ritz eigenvalues + Matrix ritz_vectors_; // all ritz eigenvectors + Vector ritz_conv_; // store whether the ritz eigenpair converged + Vector sorted_ritz_val_; // sorted converged eigenvalue + Matrix sorted_ritz_vectors_; // sorted converged eigenvectors + +public: // Constructor - explicit MatrixProdFunctor(const Sparse &A, int nev, int ncv, - double sigma = 0) + explicit PowerFunctor(const Sparse &A, int nev, int ncv, + double beta = 0) : A_(A), m_n_(A.rows()), m_nev_(nev), - m_ncv_(ncv > m_n_ ? m_n_ : ncv), - sigma_(sigma) {} + // m_ncv_(ncv > m_n_ ? m_n_ : ncv), + beta_(beta), + m_niter_(0) + {} 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()); + Vector perform_op(const Vector& x1, const Vector& x0) const { // Do the multiplication using wrapped Eigen vectors - Y = A_ * X + sigma_ * X; + Vector x2 = A_ * x1 - beta_ * x0; + x2.normalize(); + return x2; + } + + Vector perform_op(int i) const { + + // Do the multiplication using wrapped Eigen vectors + Vector x1 = ritz_vectors_.col(i-1); + Vector x2 = ritz_vectors_.col(i-2); + return perform_op(x1, x2); } 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; + // long m_rand = m_n_ ? (m_n_ & m_max) : 1; unsigned long lo, hi; lo = m_a * (long)(seed & 0xFFFF); @@ -103,62 +123,119 @@ struct MatrixProdFunctor { long m_rand = next_long_rand(m_rand); res[i] = double(m_rand) / double(m_max) - double(0.5); } + res.normalize(); return res; } - void init(Vector data) { - ritz_val_.resize(m_ncv_); + void init(const Vector x0, const Vector x00) { + // initialzie ritz eigen values + ritz_val_.resize(m_n_); ritz_val_.setZero(); - ritz_vectors_.resize(m_ncv_, m_nev_); + + // initialzie the ritz converged vector + ritz_conv_.resize(m_n_); + ritz_conv_.setZero(); + + // initialzie ritz eigen vectors + ritz_vectors_.resize(m_n_, m_n_); ritz_vectors_.setZero(); - Eigen::Map v0(init_resid.data(), m_n_); + ritz_vectors_.col(0) = perform_op(x0, x00); + ritz_vectors_.col(1) = perform_op(ritz_vectors_.col(0), x0); + + // setting beta + Vector init_resid = ritz_vectors_.col(0); + const double up = init_resid.transpose() * A_ * init_resid; + const double down = init_resid.transpose().dot(init_resid); + const double mu = up/down; + beta_ = mu*mu/4; + } void init() { - Vector init_resid = random_vec(m_n_); - init(init_resid); + Vector x0 = random_vec(m_n_); + Vector x00 = random_vec(m_n_); + init(x0, x00); } - bool converged(double tol, const Vector x) { + bool converged(double tol, int i) { + Vector x = ritz_vectors_.col(i); double theta = x.transpose() * A_ * x; + + // store the ritz eigen value + ritz_val_(i) = theta; + + // update beta + beta_ = std::max(beta_, theta * theta / 4); + Vector diff = A_ * x - theta * x; double error = diff.lpNorm<1>(); + if (error < tol) ritz_conv_(i) = 1; return error < tol; } int num_converged(double tol) { - int converge = 0; + int num_converge = 0; for (int i=0; i0 && i> pairs; + for(int i=0; i& left, const std::pair& right) { + return left.first < right.first; + }); + + // initialzie sorted ritz eigenvalues and eigenvectors + size_t num_converged = pairs.size(); + sorted_ritz_val_.resize(num_converged); + sorted_ritz_val_.setZero(); + sorted_ritz_vectors_.resize(m_n_, num_converged); + sorted_ritz_vectors_.setZero(); + + // fill sorted ritz eigenvalues and eigenvectors with sorted index + for(size_t j=0; j= 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; + // sort the result + sort_eigenpair(); return std::min(m_nev_, nconv); } Vector eigenvalues() { - return ritz_val_; + return sorted_ritz_val_; } Matrix eigenvectors() { - return ritz_vectors_; + return sorted_ritz_vectors_; } }; diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/sfm/tests/testPowerMethod.cpp index f67fad458..2e9c17345 100644 --- a/gtsam/sfm/tests/testPowerMethod.cpp +++ b/gtsam/sfm/tests/testPowerMethod.cpp @@ -18,16 +18,45 @@ * @brief Check eigenvalue and eigenvector computed by power method */ +#include +#include + #include -#include + +#include +#include +#include +#include using namespace std; using namespace gtsam; +ShonanAveraging3 fromExampleName( + const std::string &name, + ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) { + string g2oFile = findExampleDataFile(name); + return ShonanAveraging3(g2oFile, parameters); +} + +static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o"); + /* ************************************************************************* */ TEST(PowerMethod, initialize) { - gtsam::Sparse A; - MatrixProdFunctor(A, 1, A.rows()); + gtsam::Sparse A(6, 6); + A.coeffRef(0, 0) = 6; + PowerFunctor pf(A, 1, A.rows()); + pf.init(); + pf.compute(20, 1e-4); + EXPECT_LONGS_EQUAL(6, pf.eigenvectors().cols()); + EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); + + const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + Vector6 actual = pf.eigenvectors().col(0); + actual(0) = abs(actual(0)); + EXPECT(assert_equal(x1, actual)); + + const double ev1 = 6.0; + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues()(0), 1e-5); } /* ************************************************************************* */ From fc112a4dc7a3fb62bf2ba951e2b3ea650f55bc70 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 14 Sep 2020 01:59:31 -0400 Subject: [PATCH 03/22] Added PowerMinimumEigenValue method and unittest. --- gtsam/sfm/ShonanAveraging.cpp | 62 +++++++++++++++++++++---- gtsam/sfm/ShonanAveraging.h | 8 ++++ gtsam/sfm/tests/testShonanAveraging.cpp | 5 ++ 3 files changed, 67 insertions(+), 8 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index c2e2a528c..786c91890 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -480,31 +480,30 @@ static bool PowerMinimumEigenValue( Eigen::Index numLanczosVectors = 20) { // a. Compute dominant eigenpair of S using power method - MatrixProdFunctor lmOperator(A, 1, std::min(numLanczosVectors, A.rows())); + PowerFunctor lmOperator(A, 1, std::min(numLanczosVectors, A.rows())); lmOperator.init(); - const int lmConverged = lmEigenValueSolver.compute( - maxIterations, 1e-4); + const int lmConverged = lmOperator.compute( + maxIterations, 1e-5); // Check convergence and bail out if necessary if (lmConverged != 1) return false; - const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0); + const double lmEigenValue = lmOperator.eigenvalues()(0); 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 = lmOperator.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())); + Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + PowerFunctor minShiftedOperator(C, 1, std::min(numLanczosVectors, C.rows())); minShiftedOperator.init(); const int minConverged = minShiftedOperator.compute( @@ -521,6 +520,36 @@ static bool PowerMinimumEigenValue( return true; } +/** 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_; + + // Constructor + explicit MatrixProdFunctor(const Sparse &A, double sigma = 0) + : A_(A), 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; + } +}; + /// Function to compute the minimum eigenvalue of A using Lanczos in Spectra. /// This does 2 things: /// @@ -659,6 +688,23 @@ double ShonanAveraging::computeMinEigenValue(const Values &values, return minEigenValue; } +/* ************************************************************************* */ +template +double ShonanAveraging::computeMinEigenValueAP(const Values &values, + Vector *minEigenVector) const { + assert(values.size() == nrUnknowns()); + const Matrix S = StiefelElementMatrix(values); + auto A = computeA(S); + + double minEigenValue; + bool success = PowerMinimumEigenValue(A, S, &minEigenValue, minEigenVector); + if (!success) { + throw std::runtime_error( + "PowerMinimumEigenValue failed to compute minimum eigenvalue."); + } + return minEigenValue; +} + /* ************************************************************************* */ template std::pair ShonanAveraging::computeMinEigenVector( diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index edd9f33a2..9d117a266 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -202,6 +203,13 @@ class GTSAM_EXPORT ShonanAveraging { double computeMinEigenValue(const Values &values, Vector *minEigenVector = nullptr) const; + /** + * Compute minimum eigenvalue with accelerated power method. + * @param values: should be of type SOn + */ + double computeMinEigenValueAP(const Values &values, + Vector *minEigenVector = nullptr) const; + /// Project pxdN Stiefel manifold matrix S to Rot3^N Values roundSolutionS(const Matrix &S) const; diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index 1200c8ebb..4434043e8 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -166,9 +166,14 @@ TEST(ShonanAveraging3, CheckWithEigen) { for (int i = 1; i < lambdas.size(); i++) minEigenValue = min(lambdas(i), minEigenValue); + // Compute Eigenvalue with Accelerated Power method + double lambdaAP = kShonan.computeMinEigenValueAP(Qstar3); + // Actual check EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11); EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11); + EXPECT_DOUBLES_EQUAL(0, lambdaAP, 1e-11); + // Construct test descent direction (as minEigenVector is not predictable // across platforms, being one from a basically flat 3d- subspace) From d6e2546cf546d9863a5c2d1c1ffa48036f2459ff Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sat, 19 Sep 2020 23:28:43 -0400 Subject: [PATCH 04/22] feat: Add Best Heavy Ball alg to set beta --- gtsam/sfm/PowerMethod.h | 202 ++++++++++++++++++++++------ gtsam/sfm/ShonanAveraging.cpp | 4 +- gtsam/sfm/tests/testPowerMethod.cpp | 34 +++-- 3 files changed, 190 insertions(+), 50 deletions(-) diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h index 3b83fb8a4..b8471e8e2 100644 --- a/gtsam/sfm/PowerMethod.h +++ b/gtsam/sfm/PowerMethod.h @@ -30,26 +30,47 @@ #include #include #include +#include +#include +#include namespace gtsam { using Sparse = Eigen::SparseMatrix; - /* ************************************************************************* */ +/* ************************************************************************* */ /// 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 PowerFunctor { + /** + * \brief Computer i-th Eigenpair with power method + * + * References : + * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * enhancements for certifiably correct SLAM. In Proceedings of the + * International Conference on Intelligent Robots and Systems. + * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. 58–67 + * + * It performs the following iteration: \f$ x_{k+1} = A * x_k + \beta * + * x_{k-1} \f$ where A is the certificate matrix, x is the ritz vector + * + */ + // Const reference to an externally-held matrix whose minimum-eigenvalue we // want to compute const Sparse &A_; + const Matrix &S_; + const int m_n_; // dimension of Matrix A const int m_nev_; // number of eigenvalues required + // flag for running power method or accelerated power method. If false, the former, vice versa. + bool accelerated_; + // a Polyak momentum term double beta_; @@ -64,41 +85,55 @@ private: Matrix sorted_ritz_vectors_; // sorted converged eigenvectors public: - // Constructor - explicit PowerFunctor(const Sparse &A, int nev, int ncv, - double beta = 0) - : A_(A), - m_n_(A.rows()), - m_nev_(nev), - // m_ncv_(ncv > m_n_ ? m_n_ : ncv), - beta_(beta), - m_niter_(0) - {} + // Constructor + explicit PowerFunctor(const Sparse& A, const Matrix& S, int nev, int ncv, + bool accelerated = false, double beta = 0) + : A_(A), + S_(S), + m_n_(A.rows()), + m_nev_(nev), + // m_ncv_(ncv > m_n_ ? m_n_ : ncv), + accelerated_(accelerated), + beta_(beta), + m_niter_(0) { + // Do nothing + } - int rows() const { return A_.rows(); } - int cols() const { return A_.cols(); } + int rows() const { return A_.rows(); } + int cols() const { return A_.cols(); } - // Matrix-vector multiplication operation - Vector perform_op(const Vector& x1, const Vector& x0) const { + // Matrix-vector multiplication operation + Vector perform_op(const Vector& x1, const Vector& x0) const { + // Do the multiplication + Vector x2 = A_ * x1 - beta_ * x0; + x2.normalize(); + return x2; + } - // Do the multiplication using wrapped Eigen vectors - Vector x2 = A_ * x1 - beta_ * x0; + Vector perform_op(const Vector& x1, const Vector& x0, const double beta) const { + Vector x2 = A_ * x1 - beta * x0; + x2.normalize(); + return x2; + } + + Vector perform_op(const Vector& x1) const { + Vector x2 = A_ * x1; x2.normalize(); return x2; } Vector perform_op(int i) const { - - // Do the multiplication using wrapped Eigen vectors - Vector x1 = ritz_vectors_.col(i-1); - Vector x2 = ritz_vectors_.col(i-2); - return perform_op(x1, x2); + if (accelerated_) { + Vector x1 = ritz_vectors_.col(i-1); + Vector x2 = ritz_vectors_.col(i-2); + return perform_op(x1, x2); + } else + return perform_op(ritz_vectors_.col(i-1)); } 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); @@ -127,6 +162,68 @@ public: return res; } + /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) + void setBeta() { + if (m_n_ < 10) return; + double maxBeta = beta_; + size_t maxIndex; + std::vector betas = {2/3*maxBeta, 0.99*maxBeta, maxBeta, 1.01*maxBeta, 1.5*maxBeta}; + + Matrix tmp_ritz_vectors; + tmp_ritz_vectors.resize(m_n_, 10); + tmp_ritz_vectors.setZero(); + for (size_t i = 0; i < 10; i++) { + for (size_t k = 0; k < betas.size(); ++k) { + for (size_t j = 1; j < 10; j++) { + // double rayleighQuotient; + if (j <2 ) { + Vector x0 = random_vec(m_n_); + Vector x00 = random_vec(m_n_); + tmp_ritz_vectors.col(0) = perform_op(x0, x00, betas[k]); + tmp_ritz_vectors.col(1) = + perform_op(tmp_ritz_vectors.col(0), x0, betas[k]); + } + else { + tmp_ritz_vectors.col(j) = + perform_op(tmp_ritz_vectors.col(j - 1), + tmp_ritz_vectors.col(j - 2), betas[k]); + } + const Vector x = tmp_ritz_vectors.col(j); + const double up = x.transpose() * A_ * x; + const double down = x.transpose().dot(x); + const double mu = up / down; + if (mu * mu / 4 > maxBeta) { + maxIndex = k; + maxBeta = mu * mu / 4; + break; + } + } + } + } + + beta_ = betas[maxIndex]; + } + + void perturb(int i) { + // generate a 0.03*||x_0||_2 as stated in David's paper + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::mt19937 generator (seed); + std::uniform_real_distribution uniform01(0.0, 1.0); + + int n = m_n_; + Vector disturb; + disturb.resize(n); + disturb.setZero(); + for (int i =0; i(); @@ -179,7 +291,9 @@ public: if (converged(tol, i)) { num_converge += 1; } - if (i>0 && i0 && i= m_nev_) break; + else reset(); } // sort the result diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 786c91890..f287437d4 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -480,7 +480,7 @@ static bool PowerMinimumEigenValue( Eigen::Index numLanczosVectors = 20) { // a. Compute dominant eigenpair of S using power method - PowerFunctor lmOperator(A, 1, std::min(numLanczosVectors, A.rows())); + PowerFunctor lmOperator(A, S, 1, A.rows()); lmOperator.init(); const int lmConverged = lmOperator.compute( @@ -503,7 +503,7 @@ static bool PowerMinimumEigenValue( } Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; - PowerFunctor minShiftedOperator(C, 1, std::min(numLanczosVectors, C.rows())); + PowerFunctor minShiftedOperator(C, S, 1, C.rows(), true); minShiftedOperator.init(); const int minConverged = minShiftedOperator.compute( diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/sfm/tests/testPowerMethod.cpp index 2e9c17345..4547c91b9 100644 --- a/gtsam/sfm/tests/testPowerMethod.cpp +++ b/gtsam/sfm/tests/testPowerMethod.cpp @@ -41,21 +41,37 @@ ShonanAveraging3 fromExampleName( static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o"); /* ************************************************************************* */ -TEST(PowerMethod, initialize) { +TEST(PowerMethod, powerIteration) { + // test power accelerated iteration gtsam::Sparse A(6, 6); A.coeffRef(0, 0) = 6; - PowerFunctor pf(A, 1, A.rows()); - pf.init(); - pf.compute(20, 1e-4); - EXPECT_LONGS_EQUAL(6, pf.eigenvectors().cols()); - EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); + Matrix S = Matrix66::Zero(); + PowerFunctor apf(A, S, 1, A.rows(), true); + apf.init(); + apf.compute(20, 1e-4); + EXPECT_LONGS_EQUAL(6, apf.eigenvectors().cols()); + EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows()); const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); - Vector6 actual = pf.eigenvectors().col(0); - actual(0) = abs(actual(0)); - EXPECT(assert_equal(x1, actual)); + Vector6 actual0 = apf.eigenvectors().col(0); + actual0(0) = abs(actual0(0)); + EXPECT(assert_equal(x1, actual0)); const double ev1 = 6.0; + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues()(0), 1e-5); + + // test power iteration, beta is set to 0 + PowerFunctor pf(A, S, 1, A.rows()); + pf.init(); + pf.compute(20, 1e-4); + // for power method, only 5 ritz vectors converge with 20 iteration + EXPECT_LONGS_EQUAL(5, pf.eigenvectors().cols()); + EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); + + Vector6 actual1 = apf.eigenvectors().col(0); + actual1(0) = abs(actual1(0)); + EXPECT(assert_equal(x1, actual1)); + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues()(0), 1e-5); } From 758c4b061d695fabe931e0df8aa5c6c1a6ec082e Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Wed, 30 Sep 2020 11:55:18 -0400 Subject: [PATCH 05/22] Refactor power and accelerated method --- gtsam/sfm/PowerMethod.h | 420 ++++++++++------------------ gtsam/sfm/ShonanAveraging.cpp | 16 +- gtsam/sfm/tests/testPowerMethod.cpp | 71 +++-- 3 files changed, 206 insertions(+), 301 deletions(-) diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h index b8471e8e2..5fa355aaa 100644 --- a/gtsam/sfm/PowerMethod.h +++ b/gtsam/sfm/PowerMethod.h @@ -13,7 +13,8 @@ * @file PowerMethod.h * @date Sept 2020 * @author Jing Wu - * @brief accelerated power method for fast eigenvalue and eigenvector computation + * @brief accelerated power method for fast eigenvalue and eigenvector + * computation */ #pragma once @@ -24,15 +25,15 @@ #include #include +#include +#include +#include #include +#include #include #include #include #include -#include -#include -#include -#include namespace gtsam { @@ -41,9 +42,11 @@ using Sparse = Eigen::SparseMatrix; /* ************************************************************************* */ /// MINIMUM EIGENVALUE COMPUTATIONS -struct PowerFunctor { +// Template argument Operator just needs multiplication operator +template +class PowerMethod { /** - * \brief Computer i-th Eigenpair with power method + * \brief Compute maximum Eigenpair with power method * * References : * 1) Rosen, D. and Carlone, L., 2017, September. Computational @@ -52,144 +55,180 @@ struct PowerFunctor { * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated - * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. 58–67 + * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. + * 58–67 * * It performs the following iteration: \f$ x_{k+1} = A * x_k + \beta * - * x_{k-1} \f$ where A is the certificate matrix, x is the ritz vector + * x_{k-1} \f$ where A is the certificate matrix, x is the Ritz vector * */ - + public: // Const reference to an externally-held matrix whose minimum-eigenvalue we // want to compute - const Sparse &A_; + const Operator &A_; - const Matrix &S_; + const int dim_; // dimension of Matrix A - const int m_n_; // dimension of Matrix A - const int m_nev_; // number of eigenvalues required + size_t nrIterations_; // number of iterations - // flag for running power method or accelerated power method. If false, the former, vice versa. - bool accelerated_; + private: + double ritzValues_; // all Ritz eigenvalues + Vector ritzVectors_; // all Ritz eigenvectors - // a Polyak momentum term - double beta_; + public: + // Constructor + explicit PowerMethod(const Operator &A, const Vector &initial) + : A_(A), dim_(A.rows()), nrIterations_(0) { + Vector x0; + x0 = initial.isZero(0) ? Vector::Random(dim_) : initial; + x0.normalize(); - // const int m_ncv_; // dimention of orthonormal basis subspace - size_t m_niter_; // number of iterations + // initialize Ritz eigen values + ritzValues_ = 0.0; -private: - Vector ritz_val_; // all ritz eigenvalues - Matrix ritz_vectors_; // all ritz eigenvectors - Vector ritz_conv_; // store whether the ritz eigenpair converged - Vector sorted_ritz_val_; // sorted converged eigenvalue - Matrix sorted_ritz_vectors_; // sorted converged eigenvectors + // initialize Ritz eigen vectors + ritzVectors_.resize(dim_, 1); + ritzVectors_.setZero(); -public: - // Constructor - explicit PowerFunctor(const Sparse& A, const Matrix& S, int nev, int ncv, - bool accelerated = false, double beta = 0) - : A_(A), - S_(S), - m_n_(A.rows()), - m_nev_(nev), - // m_ncv_(ncv > m_n_ ? m_n_ : ncv), - accelerated_(accelerated), - beta_(beta), - m_niter_(0) { - // Do nothing - } - - int rows() const { return A_.rows(); } - int cols() const { return A_.cols(); } - - // Matrix-vector multiplication operation - Vector perform_op(const Vector& x1, const Vector& x0) const { - // Do the multiplication - Vector x2 = A_ * x1 - beta_ * x0; - x2.normalize(); - return x2; + ritzVectors_.col(0) = update(x0); + perturb(); } - Vector perform_op(const Vector& x1, const Vector& x0, const double beta) const { - Vector x2 = A_ * x1 - beta * x0; - x2.normalize(); - return x2; + Vector update(const Vector &x) const { + Vector y = A_ * x; + y.normalize(); + return y; } - Vector perform_op(const Vector& x1) const { - Vector x2 = A_ * x1; - x2.normalize(); - return x2; - } + Vector update() const { return update(ritzVectors_); } - Vector perform_op(int i) const { - if (accelerated_) { - Vector x1 = ritz_vectors_.col(i-1); - Vector x2 = ritz_vectors_.col(i-2); - return perform_op(x1, x2); - } else - return perform_op(ritz_vectors_.col(i-1)); - } + void updateRitz(const Vector &ritz) { ritzVectors_ = ritz; } - long next_long_rand(long seed) { - const unsigned int m_a = 16807; - const unsigned long m_max = 2147483647L; - unsigned long lo, hi; + Vector getRitz() { return ritzVectors_; } - 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; + void perturb() { + // generate a 0.03*||x_0||_2 as stated in David's paper + unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); + std::mt19937 generator(seed); + std::uniform_real_distribution uniform01(0.0, 1.0); + + int n = dim_; + Vector disturb; + disturb.resize(n); + disturb.setZero(); + for (int i = 0; i < n; ++i) { + disturb(i) = uniform01(generator); } - lo += hi >> 15; - if (lo > m_max) { - lo &= m_max; - ++lo; - } - return (long)lo; + disturb.normalize(); + + Vector x0 = ritzVectors_; + double magnitude = x0.norm(); + ritzVectors_ = x0 + 0.03 * magnitude * disturb; } - 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); + // Perform power iteration on a single Ritz value + // Updates ritzValues_ + bool iterateOne(double tol) { + const Vector x = ritzVectors_; + double theta = x.transpose() * A_ * x; + + // store the Ritz eigen value + ritzValues_ = theta; + + const Vector diff = A_ * x - theta * x; + double error = diff.norm(); + return error < tol; + } + + size_t nrIterations() { return nrIterations_; } + + int compute(int maxit, double tol) { + // Starting + int nrConverged = 0; + + for (int i = 0; i < maxit; i++) { + nrIterations_ += 1; + ritzVectors_ = update(); + nrConverged = iterateOne(tol); + if (nrConverged) break; } - res.normalize(); - return res; + + return std::min(1, nrConverged); + } + + double eigenvalues() { return ritzValues_; } + + Vector eigenvectors() { return ritzVectors_; } +}; + +template +class AcceleratedPowerMethod : public PowerMethod { + double beta_ = 0; // a Polyak momentum term + + Vector previousVector_; // store previous vector + + public: + // Constructor + explicit AcceleratedPowerMethod(const Operator &A, const Vector &initial) + : PowerMethod(A, initial) { + Vector x0 = initial; + // initialize ritz vector + x0 = x0.isZero(0) ? Vector::Random(PowerMethod::dim_) : x0; + Vector x00 = Vector::Random(PowerMethod::dim_); + x0.normalize(); + x00.normalize(); + + // initialize Ritz eigen vector and previous vector + previousVector_ = update(x0, x00, beta_); + this->updateRitz(update(previousVector_, x0, beta_)); + this->perturb(); + + // set beta + Vector init_resid = this->getRitz(); + const double up = init_resid.transpose() * this->A_ * init_resid; + const double down = init_resid.transpose().dot(init_resid); + const double mu = up / down; + beta_ = mu * mu / 4; + setBeta(); + } + + Vector update(const Vector &x1, const Vector &x0, const double beta) const { + Vector y = this->A_ * x1 - beta * x0; + y.normalize(); + return y; + } + + Vector update() const { + Vector y = update(this->ritzVectors_, previousVector_, beta_); + previousVector_ = this->ritzVectors_; + return y; } /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) void setBeta() { - if (m_n_ < 10) return; + if (PowerMethod::dim_ < 10) return; double maxBeta = beta_; size_t maxIndex; - std::vector betas = {2/3*maxBeta, 0.99*maxBeta, maxBeta, 1.01*maxBeta, 1.5*maxBeta}; + std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, + 1.01 * maxBeta, 1.5 * maxBeta}; - Matrix tmp_ritz_vectors; - tmp_ritz_vectors.resize(m_n_, 10); - tmp_ritz_vectors.setZero(); + Matrix tmpRitzVectors; + tmpRitzVectors.resize(PowerMethod::dim_, 10); + tmpRitzVectors.setZero(); for (size_t i = 0; i < 10; i++) { for (size_t k = 0; k < betas.size(); ++k) { for (size_t j = 1; j < 10; j++) { - // double rayleighQuotient; - if (j <2 ) { - Vector x0 = random_vec(m_n_); - Vector x00 = random_vec(m_n_); - tmp_ritz_vectors.col(0) = perform_op(x0, x00, betas[k]); - tmp_ritz_vectors.col(1) = - perform_op(tmp_ritz_vectors.col(0), x0, betas[k]); + if (j < 2) { + Vector x0 = Vector::Random(PowerMethod::dim_); + Vector x00 = Vector::Random(PowerMethod::dim_); + tmpRitzVectors.col(0) = update(x0, x00, betas[k]); + tmpRitzVectors.col(1) = update(tmpRitzVectors.col(0), x0, betas[k]); + } else { + tmpRitzVectors.col(j) = update(tmpRitzVectors.col(j - 1), + tmpRitzVectors.col(j - 2), betas[k]); } - else { - tmp_ritz_vectors.col(j) = - perform_op(tmp_ritz_vectors.col(j - 1), - tmp_ritz_vectors.col(j - 2), betas[k]); - } - const Vector x = tmp_ritz_vectors.col(j); - const double up = x.transpose() * A_ * x; + const Vector x = tmpRitzVectors.col(j); + const double up = x.transpose() * this->A_ * x; const double down = x.transpose().dot(x); const double mu = up / down; if (mu * mu / 4 > maxBeta) { @@ -203,165 +242,6 @@ public: beta_ = betas[maxIndex]; } - - void perturb(int i) { - // generate a 0.03*||x_0||_2 as stated in David's paper - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::mt19937 generator (seed); - std::uniform_real_distribution uniform01(0.0, 1.0); - - int n = m_n_; - Vector disturb; - disturb.resize(n); - disturb.setZero(); - for (int i =0; i(); - if (error < tol) ritz_conv_(i) = 1; - return error < tol; - } - - int num_converged(double tol) { - int num_converge = 0; - for (int i=0; i0 && i> pairs; - for(int i=0; i& left, const std::pair& right) { - return left.first < right.first; - }); - - // initialzie sorted ritz eigenvalues and eigenvectors - size_t num_converged = pairs.size(); - sorted_ritz_val_.resize(num_converged); - sorted_ritz_val_.setZero(); - sorted_ritz_vectors_.resize(m_n_, num_converged); - sorted_ritz_vectors_.setZero(); - - // fill sorted ritz eigenvalues and eigenvectors with sorted index - for(size_t j=0; j= m_nev_) break; - else reset(); - } - - // sort the result - sort_eigenpair(); - - return std::min(m_nev_, nconv); - } - - Vector eigenvalues() { - return sorted_ritz_val_; - } - - Matrix eigenvectors() { - return sorted_ritz_vectors_; - } - }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index f287437d4..9ebee4f9f 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -480,8 +480,7 @@ static bool PowerMinimumEigenValue( Eigen::Index numLanczosVectors = 20) { // a. Compute dominant eigenpair of S using power method - PowerFunctor lmOperator(A, S, 1, A.rows()); - lmOperator.init(); + PowerMethod lmOperator(A, S.row(0)); const int lmConverged = lmOperator.compute( maxIterations, 1e-5); @@ -489,34 +488,33 @@ static bool PowerMinimumEigenValue( // Check convergence and bail out if necessary if (lmConverged != 1) return false; - const double lmEigenValue = lmOperator.eigenvalues()(0); + const double lmEigenValue = lmOperator.eigenvalues(); 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 = lmOperator.eigenvectors().col(0); + *minEigenVector = lmOperator.eigenvectors(); minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; - PowerFunctor minShiftedOperator(C, S, 1, C.rows(), true); - minShiftedOperator.init(); + AcceleratedPowerMethod minShiftedOperator(C, S.row(0)); const int minConverged = minShiftedOperator.compute( maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); if (minConverged != 1) return false; - *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues()(0); + *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues(); if (minEigenVector) { - *minEigenVector = minShiftedOperator.eigenvectors().col(0); + *minEigenVector = minShiftedOperator.eigenvectors(); minEigenVector->normalize(); // Ensure that this is a unit vector } - if (numIterations) *numIterations = minShiftedOperator.num_iterations(); + if (numIterations) *numIterations = minShiftedOperator.nrIterations(); return true; } diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/sfm/tests/testPowerMethod.cpp index 4547c91b9..d02906980 100644 --- a/gtsam/sfm/tests/testPowerMethod.cpp +++ b/gtsam/sfm/tests/testPowerMethod.cpp @@ -18,38 +18,34 @@ * @brief Check eigenvalue and eigenvector computed by power method */ +#include +#include +#include +#include #include -#include #include #include #include +#include + #include #include using namespace std; using namespace gtsam; - -ShonanAveraging3 fromExampleName( - const std::string &name, - ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) { - string g2oFile = findExampleDataFile(name); - return ShonanAveraging3(g2oFile, parameters); -} - -static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o"); +using symbol_shorthand::X; /* ************************************************************************* */ TEST(PowerMethod, powerIteration) { - // test power accelerated iteration - gtsam::Sparse A(6, 6); + // test power iteration, beta is set to 0 + Sparse A(6, 6); A.coeffRef(0, 0) = 6; Matrix S = Matrix66::Zero(); - PowerFunctor apf(A, S, 1, A.rows(), true); - apf.init(); + PowerMethod apf(A, S.row(0)); apf.compute(20, 1e-4); - EXPECT_LONGS_EQUAL(6, apf.eigenvectors().cols()); + EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols()); EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows()); const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); @@ -58,21 +54,52 @@ TEST(PowerMethod, powerIteration) { EXPECT(assert_equal(x1, actual0)); const double ev1 = 6.0; - EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues()(0), 1e-5); + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-5); - // test power iteration, beta is set to 0 - PowerFunctor pf(A, S, 1, A.rows()); - pf.init(); + // test power accelerated iteration + AcceleratedPowerMethod pf(A, S.row(0)); pf.compute(20, 1e-4); - // for power method, only 5 ritz vectors converge with 20 iteration - EXPECT_LONGS_EQUAL(5, pf.eigenvectors().cols()); + // for power method, only 5 ritz vectors converge with 20 iterations + EXPECT_LONGS_EQUAL(1, pf.eigenvectors().cols()); EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); Vector6 actual1 = apf.eigenvectors().col(0); actual1(0) = abs(actual1(0)); EXPECT(assert_equal(x1, actual1)); - EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues()(0), 1e-5); + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-5); +} + +/* ************************************************************************* */ +TEST(PowerMethod, useFactorGraph) { + // Let's make a scalar synchronization graph with 4 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + for (size_t j = 0; j < 3; j++) { + fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); + } + fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + + // Get eigenvalues and eigenvectors with Eigen + auto L = fg.hessian(); + cout << L.first << endl; + Eigen::EigenSolver solver(L.first); + cout << solver.eigenvalues() << endl; + cout << solver.eigenvectors() << endl; + + // Check that we get zero eigenvalue and "constant" eigenvector + EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); + auto v0 = solver.eigenvectors().col(0); + for (size_t j = 0; j < 3; j++) + EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + + // test power iteration, beta is set to 0 + Matrix S = Matrix44::Zero(); + // PowerMethod pf(L.first, S.row(0)); + AcceleratedPowerMethod pf(L.first, S.row(0)); + pf.compute(20, 1e-4); + cout << pf.eigenvalues() << endl; + cout << pf.eigenvectors() << endl; } /* ************************************************************************* */ From b7f29a051a4b8f77c6bdd1e65490c579db0c1272 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 13 Oct 2020 02:34:24 -0400 Subject: [PATCH 06/22] Reformat code. --- gtsam/linear/AcceleratedPowerMethod.h | 135 ++++++++++ gtsam/linear/PowerMethod.h | 140 ++++++++++ .../tests/testAcceleratedPowerMethod.cpp | 107 ++++++++ .../{sfm => linear}/tests/testPowerMethod.cpp | 55 ++-- gtsam/sfm/PowerMethod.h | 247 ------------------ gtsam/sfm/ShonanAveraging.cpp | 19 +- gtsam/sfm/ShonanAveraging.h | 3 +- 7 files changed, 422 insertions(+), 284 deletions(-) create mode 100644 gtsam/linear/AcceleratedPowerMethod.h create mode 100644 gtsam/linear/PowerMethod.h create mode 100644 gtsam/linear/tests/testAcceleratedPowerMethod.cpp rename gtsam/{sfm => linear}/tests/testPowerMethod.cpp (70%) delete mode 100644 gtsam/sfm/PowerMethod.h diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h new file mode 100644 index 000000000..2cbcaf67e --- /dev/null +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -0,0 +1,135 @@ +/* ---------------------------------------------------------------------------- + + * 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 AcceleratedPowerMethod.h + * @date Sept 2020 + * @author Jing Wu + * @brief accelerated power method for fast eigenvalue and eigenvector + * computation + */ + +#pragma once + +// #include +// #include +#include + +namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + +/* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS + +// Template argument Operator just needs multiplication operator +template +class AcceleratedPowerMethod : public PowerMethod { + /** + * \brief Compute maximum Eigenpair with power method + * + * References : + * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * enhancements for certifiably correct SLAM. In Proceedings of the + * International Conference on Intelligent Robots and Systems. + * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. + * 58–67 + * + * It performs the following iteration: \f$ x_{k+1} = A * x_k + \beta * + * x_{k-1} \f$ where A is the certificate matrix, x is the Ritz vector + * + */ + + double beta_ = 0; // a Polyak momentum term + + Vector previousVector_; // store previous vector + + public: + // Constructor from aim matrix A (given as Matrix or Sparse), optional intial + // vector as ritzVector + explicit AcceleratedPowerMethod( + const Operator &A, const boost::optional initial = boost::none) + : PowerMethod(A, initial) { + Vector x0; + // initialize ritz vector + x0 = initial ? Vector::Random(this->dim_) : initial.get(); + Vector x00 = Vector::Random(this->dim_); + x0.normalize(); + x00.normalize(); + + // initialize Ritz eigen vector and previous vector + previousVector_ = update(x0, x00, beta_); + this->updateRitz(update(previousVector_, x0, beta_)); + this->ritzVector_ = update(previousVector_, x0, beta_); + // this->updateRitz(update(previousVector_, x0, beta_)); + this->perturb(); + + // set beta + Vector init_resid = this->ritzVector_; + const double up = init_resid.transpose() * this->A_ * init_resid; + const double down = init_resid.transpose().dot(init_resid); + const double mu = up / down; + beta_ = mu * mu / 4; + setBeta(); + } + + // Update the ritzVector with beta and previous two ritzVector + Vector update(const Vector &x1, const Vector &x0, const double beta) const { + Vector y = this->A_ * x1 - beta * x0; + y.normalize(); + return y; + } + + // Update the ritzVector with beta and previous two ritzVector + Vector update() const { + Vector y = update(this->ritzVector_, previousVector_, beta_); + previousVector_ = this->ritzVector_; + return y; + } + + // Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) + void setBeta() { + double maxBeta = beta_; + size_t maxIndex; + std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, + 1.01 * maxBeta, 1.5 * maxBeta}; + + Matrix R = Matrix::Zero(this->dim_, 10); + for (size_t i = 0; i < 10; i++) { + for (size_t k = 0; k < betas.size(); ++k) { + for (size_t j = 1; j < 10; j++) { + if (j < 2) { + Vector x0 = this->ritzVector_; + Vector x00 = previousVector_; + R.col(0) = update(x0, x00, betas[k]); + R.col(1) = update(R.col(0), x0, betas[k]); + } else { + R.col(j) = update(R.col(j - 1), R.col(j - 2), betas[k]); + } + } + const Vector x = R.col(9); + const double up = x.transpose() * this->A_ * x; + const double down = x.transpose().dot(x); + const double mu = up / down; + if (mu * mu / 4 > maxBeta) { + maxIndex = k; + maxBeta = mu * mu / 4; + } + } + } + beta_ = betas[maxIndex]; + } +}; + +} // namespace gtsam diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h new file mode 100644 index 000000000..fe9aabb46 --- /dev/null +++ b/gtsam/linear/PowerMethod.h @@ -0,0 +1,140 @@ +/* ---------------------------------------------------------------------------- + + * 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 Power method for fast eigenvalue and eigenvector + * computation + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + +/* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS + +// Template argument Operator just needs multiplication operator +template +class PowerMethod { + protected: + // Const reference to an externally-held matrix whose minimum-eigenvalue we + // want to compute + const Operator &A_; + + const int dim_; // dimension of Matrix A + + size_t nrIterations_; // number of iterations + + double ritzValue_; // all Ritz eigenvalues + Vector ritzVector_; // all Ritz eigenvectors + + public: + // Constructor + explicit PowerMethod(const Operator &A, + const boost::optional initial = boost::none) + : A_(A), dim_(A.rows()), nrIterations_(0) { + Vector x0; + x0 = initial ? Vector::Random(dim_) : initial.get(); + x0.normalize(); + + // initialize Ritz eigen values + ritzValue_ = 0.0; + + // initialize Ritz eigen vectors + ritzVector_ = Vector::Zero(dim_); + + ritzVector_.col(0) = update(x0); + perturb(); + } + + // Update the vector by dot product with A_ + Vector update(const Vector &x) const { + Vector y = A_ * x; + y.normalize(); + return y; + } + + Vector update() const { return update(ritzVector_); } + + // Update the ritzVector_ + void updateRitz(const Vector &ritz) { ritzVector_ = ritz; } + + // Perturb the initial ritzvector + void perturb() { + // generate a 0.03*||x_0||_2 as stated in David's paper + std::mt19937 rng(42); + std::uniform_real_distribution uniform01(0.0, 1.0); + + int n = dim_; + Vector disturb(n); + for (int i = 0; i < n; ++i) { + disturb(i) = uniform01(rng); + } + disturb.normalize(); + + Vector x0 = ritzVector_; + double magnitude = x0.norm(); + ritzVector_ = x0 + 0.03 * magnitude * disturb; + } + + // Perform power iteration on a single Ritz value + // Updates ritzValue_ + bool iterateOne(double tol) { + const Vector x = ritzVector_; + double theta = x.transpose() * A_ * x; + + // store the Ritz eigen value + ritzValue_ = theta; + + const Vector diff = A_ * x - theta * x; + double error = diff.norm(); + return error < tol; + } + + // Return the number of iterations + size_t nrIterations() const { return nrIterations_; } + + // Start the iteration until the ritz error converge + int compute(int maxIterations, double tol) { + // Starting + int nrConverged = 0; + + for (int i = 0; i < maxIterations; i++) { + nrIterations_ += 1; + ritzVector_ = update(); + nrConverged = iterateOne(tol); + if (nrConverged) break; + } + + return std::min(1, nrConverged); + } + + // Return the eigenvalue + double eigenvalues() const { return ritzValue_; } + + // Return the eigenvector + const Vector eigenvectors() const { return ritzVector_; } +}; + +} // namespace gtsam diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp new file mode 100644 index 000000000..2a117d53b --- /dev/null +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * 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 testAcceleratedPowerMethod.cpp + * @date Sept 2020 + * @author Jing Wu + * @brief Check eigenvalue and eigenvector computed by accelerated power method + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::X; + +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { + // test power iteration, beta is set to 0 + Sparse A(6, 6); + A.coeffRef(0, 0) = 6; + A.coeffRef(0, 0) = 5; + A.coeffRef(0, 0) = 4; + A.coeffRef(0, 0) = 3; + A.coeffRef(0, 0) = 2; + A.coeffRef(0, 0) = 1; + Vector initial = Vector6::Zero(); + const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + const double ev1 = 1.0; + + // test accelerated power iteration + AcceleratedPowerMethod apf(A, initial); + apf.compute(20, 1e-4); + EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols()); + EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows()); + + Vector6 actual1 = apf.eigenvectors(); + // actual1(0) = abs (actual1(0)); + EXPECT(assert_equal(x1, actual1)); + + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-5); +} + +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, useFactorGraph) { + // Let's make a scalar synchronization graph with 4 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + for (size_t j = 0; j < 3; j++) { + fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); + } + fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + + // Get eigenvalues and eigenvectors with Eigen + auto L = fg.hessian(); + Eigen::EigenSolver solver(L.first); + + // Check that we get zero eigenvalue and "constant" eigenvector + EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); + auto v0 = solver.eigenvectors().col(0); + for (size_t j = 0; j < 3; j++) + EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + + size_t maxIdx = 0; + for (auto i =0; i= solver.eigenvalues()(maxIdx).real()) maxIdx = i; + } + // Store the max eigenvalue and its according eigenvector + const auto ev1 = solver.eigenvalues()(maxIdx).real(); + auto ev2 = solver.eigenvectors().col(maxIdx).real(); + + Vector initial = Vector4::Zero(); + AcceleratedPowerMethod apf(L.first, initial); + apf.compute(20, 1e-4); + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-8); + EXPECT(assert_equal(ev2, apf.eigenvectors(), 3e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp similarity index 70% rename from gtsam/sfm/tests/testPowerMethod.cpp rename to gtsam/linear/tests/testPowerMethod.cpp index d02906980..621286c85 100644 --- a/gtsam/sfm/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include @@ -42,31 +42,22 @@ TEST(PowerMethod, powerIteration) { // test power iteration, beta is set to 0 Sparse A(6, 6); A.coeffRef(0, 0) = 6; - Matrix S = Matrix66::Zero(); - PowerMethod apf(A, S.row(0)); - apf.compute(20, 1e-4); - EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols()); - EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows()); - - const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); - Vector6 actual0 = apf.eigenvectors().col(0); - actual0(0) = abs(actual0(0)); - EXPECT(assert_equal(x1, actual0)); - - const double ev1 = 6.0; - EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-5); - - // test power accelerated iteration - AcceleratedPowerMethod pf(A, S.row(0)); + A.coeffRef(0, 0) = 5; + A.coeffRef(0, 0) = 4; + A.coeffRef(0, 0) = 3; + A.coeffRef(0, 0) = 2; + A.coeffRef(0, 0) = 1; + Vector initial = Vector6::Zero(); + PowerMethod pf(A, initial); pf.compute(20, 1e-4); - // for power method, only 5 ritz vectors converge with 20 iterations EXPECT_LONGS_EQUAL(1, pf.eigenvectors().cols()); EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); - Vector6 actual1 = apf.eigenvectors().col(0); - actual1(0) = abs(actual1(0)); - EXPECT(assert_equal(x1, actual1)); + const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + Vector6 actual0 = pf.eigenvectors(); + EXPECT(assert_equal(x1, actual0)); + const double ev1 = 1.0; EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-5); } @@ -82,10 +73,7 @@ TEST(PowerMethod, useFactorGraph) { // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); - cout << L.first << endl; Eigen::EigenSolver solver(L.first); - cout << solver.eigenvalues() << endl; - cout << solver.eigenvectors() << endl; // Check that we get zero eigenvalue and "constant" eigenvector EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); @@ -93,13 +81,20 @@ TEST(PowerMethod, useFactorGraph) { for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); - // test power iteration, beta is set to 0 - Matrix S = Matrix44::Zero(); - // PowerMethod pf(L.first, S.row(0)); - AcceleratedPowerMethod pf(L.first, S.row(0)); + size_t maxIdx = 0; + for (auto i =0; i= solver.eigenvalues()(maxIdx).real()) maxIdx = i; + } + // Store the max eigenvalue and its according eigenvector + const auto ev1 = solver.eigenvalues()(maxIdx).real(); + auto ev2 = solver.eigenvectors().col(maxIdx).real(); + + Vector initial = Vector4::Zero(); + PowerMethod pf(L.first, initial); pf.compute(20, 1e-4); - cout << pf.eigenvalues() << endl; - cout << pf.eigenvectors() << endl; + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-8); + // auto actual2 = pf.eigenvectors(); + // EXPECT(assert_equal(ev2, actual2, 3e-5)); } /* ************************************************************************* */ diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h deleted file mode 100644 index 5fa355aaa..000000000 --- a/gtsam/sfm/PowerMethod.h +++ /dev/null @@ -1,247 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 -#include -#include -#include - -namespace gtsam { - -using Sparse = Eigen::SparseMatrix; - -/* ************************************************************************* */ -/// MINIMUM EIGENVALUE COMPUTATIONS - -// Template argument Operator just needs multiplication operator -template -class PowerMethod { - /** - * \brief Compute maximum Eigenpair with power method - * - * References : - * 1) Rosen, D. and Carlone, L., 2017, September. Computational - * enhancements for certifiably correct SLAM. In Proceedings of the - * International Conference on Intelligent Robots and Systems. - * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, - * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv - * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated - * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. - * 58–67 - * - * It performs the following iteration: \f$ x_{k+1} = A * x_k + \beta * - * x_{k-1} \f$ where A is the certificate matrix, x is the Ritz vector - * - */ - public: - // Const reference to an externally-held matrix whose minimum-eigenvalue we - // want to compute - const Operator &A_; - - const int dim_; // dimension of Matrix A - - size_t nrIterations_; // number of iterations - - private: - double ritzValues_; // all Ritz eigenvalues - Vector ritzVectors_; // all Ritz eigenvectors - - public: - // Constructor - explicit PowerMethod(const Operator &A, const Vector &initial) - : A_(A), dim_(A.rows()), nrIterations_(0) { - Vector x0; - x0 = initial.isZero(0) ? Vector::Random(dim_) : initial; - x0.normalize(); - - // initialize Ritz eigen values - ritzValues_ = 0.0; - - // initialize Ritz eigen vectors - ritzVectors_.resize(dim_, 1); - ritzVectors_.setZero(); - - ritzVectors_.col(0) = update(x0); - perturb(); - } - - Vector update(const Vector &x) const { - Vector y = A_ * x; - y.normalize(); - return y; - } - - Vector update() const { return update(ritzVectors_); } - - void updateRitz(const Vector &ritz) { ritzVectors_ = ritz; } - - Vector getRitz() { return ritzVectors_; } - - void perturb() { - // generate a 0.03*||x_0||_2 as stated in David's paper - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::mt19937 generator(seed); - std::uniform_real_distribution uniform01(0.0, 1.0); - - int n = dim_; - Vector disturb; - disturb.resize(n); - disturb.setZero(); - for (int i = 0; i < n; ++i) { - disturb(i) = uniform01(generator); - } - disturb.normalize(); - - Vector x0 = ritzVectors_; - double magnitude = x0.norm(); - ritzVectors_ = x0 + 0.03 * magnitude * disturb; - } - - // Perform power iteration on a single Ritz value - // Updates ritzValues_ - bool iterateOne(double tol) { - const Vector x = ritzVectors_; - double theta = x.transpose() * A_ * x; - - // store the Ritz eigen value - ritzValues_ = theta; - - const Vector diff = A_ * x - theta * x; - double error = diff.norm(); - return error < tol; - } - - size_t nrIterations() { return nrIterations_; } - - int compute(int maxit, double tol) { - // Starting - int nrConverged = 0; - - for (int i = 0; i < maxit; i++) { - nrIterations_ += 1; - ritzVectors_ = update(); - nrConverged = iterateOne(tol); - if (nrConverged) break; - } - - return std::min(1, nrConverged); - } - - double eigenvalues() { return ritzValues_; } - - Vector eigenvectors() { return ritzVectors_; } -}; - -template -class AcceleratedPowerMethod : public PowerMethod { - double beta_ = 0; // a Polyak momentum term - - Vector previousVector_; // store previous vector - - public: - // Constructor - explicit AcceleratedPowerMethod(const Operator &A, const Vector &initial) - : PowerMethod(A, initial) { - Vector x0 = initial; - // initialize ritz vector - x0 = x0.isZero(0) ? Vector::Random(PowerMethod::dim_) : x0; - Vector x00 = Vector::Random(PowerMethod::dim_); - x0.normalize(); - x00.normalize(); - - // initialize Ritz eigen vector and previous vector - previousVector_ = update(x0, x00, beta_); - this->updateRitz(update(previousVector_, x0, beta_)); - this->perturb(); - - // set beta - Vector init_resid = this->getRitz(); - const double up = init_resid.transpose() * this->A_ * init_resid; - const double down = init_resid.transpose().dot(init_resid); - const double mu = up / down; - beta_ = mu * mu / 4; - setBeta(); - } - - Vector update(const Vector &x1, const Vector &x0, const double beta) const { - Vector y = this->A_ * x1 - beta * x0; - y.normalize(); - return y; - } - - Vector update() const { - Vector y = update(this->ritzVectors_, previousVector_, beta_); - previousVector_ = this->ritzVectors_; - return y; - } - - /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) - void setBeta() { - if (PowerMethod::dim_ < 10) return; - double maxBeta = beta_; - size_t maxIndex; - std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, - 1.01 * maxBeta, 1.5 * maxBeta}; - - Matrix tmpRitzVectors; - tmpRitzVectors.resize(PowerMethod::dim_, 10); - tmpRitzVectors.setZero(); - for (size_t i = 0; i < 10; i++) { - for (size_t k = 0; k < betas.size(); ++k) { - for (size_t j = 1; j < 10; j++) { - if (j < 2) { - Vector x0 = Vector::Random(PowerMethod::dim_); - Vector x00 = Vector::Random(PowerMethod::dim_); - tmpRitzVectors.col(0) = update(x0, x00, betas[k]); - tmpRitzVectors.col(1) = update(tmpRitzVectors.col(0), x0, betas[k]); - } else { - tmpRitzVectors.col(j) = update(tmpRitzVectors.col(j - 1), - tmpRitzVectors.col(j - 2), betas[k]); - } - const Vector x = tmpRitzVectors.col(j); - const double up = x.transpose() * this->A_ * x; - const double down = x.transpose().dot(x); - const double mu = up / down; - if (mu * mu / 4 > maxBeta) { - maxIndex = k; - maxBeta = mu * mu / 4; - break; - } - } - } - } - - beta_ = betas[maxIndex]; - } -}; - -} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 9ebee4f9f..5f509c91e 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -471,16 +471,23 @@ Sparse ShonanAveraging::computeA(const Values &values) const { return Lambda - Q_; } -// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization +/* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS +// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization, +// it takes in the certificate matrix A as input, the maxIterations and the +// minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default, +// there are two parts +// in this algorithm: +// (1) 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) { + double minEigenvalueNonnegativityTolerance = 10e-4) { // a. Compute dominant eigenpair of S using power method - PowerMethod lmOperator(A, S.row(0)); + const boost::optional initial(S.row(0)); + PowerMethod lmOperator(A, initial); const int lmConverged = lmOperator.compute( maxIterations, 1e-5); @@ -501,8 +508,8 @@ static bool PowerMinimumEigenValue( return true; } - Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; - AcceleratedPowerMethod minShiftedOperator(C, S.row(0)); + const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + AcceleratedPowerMethod minShiftedOperator(C, initial); const int minConverged = minShiftedOperator.compute( maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 9d117a266..49659386f 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -26,7 +26,8 @@ #include #include #include -#include +#include +#include #include #include From fbebd3ed6954fed7766c31a172e8761cc0c56ef7 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 13 Oct 2020 12:00:25 -0400 Subject: [PATCH 07/22] Small fix --- gtsam/linear/AcceleratedPowerMethod.h | 2 -- gtsam/linear/PowerMethod.h | 13 ++++++------- gtsam/linear/tests/testAcceleratedPowerMethod.cpp | 3 +++ 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 2cbcaf67e..0e47c6a53 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -70,9 +70,7 @@ class AcceleratedPowerMethod : public PowerMethod { // initialize Ritz eigen vector and previous vector previousVector_ = update(x0, x00, beta_); - this->updateRitz(update(previousVector_, x0, beta_)); this->ritzVector_ = update(previousVector_, x0, beta_); - // this->updateRitz(update(previousVector_, x0, beta_)); this->perturb(); // set beta diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index fe9aabb46..4807e0e6a 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -75,11 +75,9 @@ class PowerMethod { return y; } + // Update the vector by dot product with A_ Vector update() const { return update(ritzVector_); } - // Update the ritzVector_ - void updateRitz(const Vector &ritz) { ritzVector_ = ritz; } - // Perturb the initial ritzvector void perturb() { // generate a 0.03*||x_0||_2 as stated in David's paper @@ -87,10 +85,11 @@ class PowerMethod { std::uniform_real_distribution uniform01(0.0, 1.0); int n = dim_; - Vector disturb(n); - for (int i = 0; i < n; ++i) { - disturb(i) = uniform01(rng); - } + // Vector disturb(n); + // for (int i = 0; i < n; ++i) { + // disturb(i) = uniform01(rng); + // } + Vector disturb = Vector::Random(n); disturb.normalize(); Vector x0 = ritzVector_; diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index 2a117d53b..a84a31a0f 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -76,7 +76,10 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); + cout << L.first << endl; Eigen::EigenSolver solver(L.first); + cout << solver.eigenvalues() << endl; + cout << solver.eigenvectors() << endl; // Check that we get zero eigenvalue and "constant" eigenvector EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); From 5f50e740b746573e6b85e1c300e6f50c9c593828 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Thu, 22 Oct 2020 14:19:05 -0400 Subject: [PATCH 08/22] Revise according to Frank and David's comments --- gtsam/linear/AcceleratedPowerMethod.h | 53 +++++++++++--------- gtsam/linear/PowerMethod.h | 72 +++++++++------------------ gtsam/sfm/ShonanAveraging.cpp | 61 +++++++++++++++-------- 3 files changed, 94 insertions(+), 92 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0e47c6a53..0ad87626b 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -34,7 +34,7 @@ using Sparse = Eigen::SparseMatrix; template class AcceleratedPowerMethod : public PowerMethod { /** - * \brief Compute maximum Eigenpair with power method + * \brief Compute maximum Eigenpair with accelerated power method * * References : * 1) Rosen, D. and Carlone, L., 2017, September. Computational @@ -46,8 +46,9 @@ class AcceleratedPowerMethod : public PowerMethod { * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. * 58–67 * - * It performs the following iteration: \f$ x_{k+1} = A * x_k + \beta * - * x_{k-1} \f$ where A is the certificate matrix, x is the Ritz vector + * It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta * + * x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the + * Ritz vector * */ @@ -59,46 +60,52 @@ class AcceleratedPowerMethod : public PowerMethod { // Constructor from aim matrix A (given as Matrix or Sparse), optional intial // vector as ritzVector explicit AcceleratedPowerMethod( - const Operator &A, const boost::optional initial = boost::none) + const Operator &A, const boost::optional initial = boost::none, + const double initialBeta = 0.0) : PowerMethod(A, initial) { Vector x0; // initialize ritz vector - x0 = initial ? Vector::Random(this->dim_) : initial.get(); + x0 = initial ? initial.get() : Vector::Random(this->dim_); Vector x00 = Vector::Random(this->dim_); x0.normalize(); x00.normalize(); // initialize Ritz eigen vector and previous vector - previousVector_ = update(x0, x00, beta_); - this->ritzVector_ = update(previousVector_, x0, beta_); - this->perturb(); + previousVector_ = powerIteration(x0, x00, beta_); + this->ritzVector_ = powerIteration(previousVector_, x0, beta_); - // set beta - Vector init_resid = this->ritzVector_; - const double up = init_resid.transpose() * this->A_ * init_resid; - const double down = init_resid.transpose().dot(init_resid); - const double mu = up / down; - beta_ = mu * mu / 4; - setBeta(); + // initialize beta_ + if (!initialBeta) { + estimateBeta(); + } else { + beta_ = initialBeta; + } + } // Update the ritzVector with beta and previous two ritzVector - Vector update(const Vector &x1, const Vector &x0, const double beta) const { + Vector powerIteration(const Vector &x1, const Vector &x0, + const double beta) const { Vector y = this->A_ * x1 - beta * x0; y.normalize(); return y; } // Update the ritzVector with beta and previous two ritzVector - Vector update() const { - Vector y = update(this->ritzVector_, previousVector_, beta_); + Vector powerIteration() const { + Vector y = powerIteration(this->ritzVector_, previousVector_, beta_); previousVector_ = this->ritzVector_; return y; } // Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) - void setBeta() { - double maxBeta = beta_; + void estimateBeta() { + // set beta + Vector init_resid = this->ritzVector_; + const double up = init_resid.transpose() * this->A_ * init_resid; + const double down = init_resid.transpose().dot(init_resid); + const double mu = up / down; + double maxBeta = mu * mu / 4; size_t maxIndex; std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta, 1.5 * maxBeta}; @@ -110,10 +117,10 @@ class AcceleratedPowerMethod : public PowerMethod { if (j < 2) { Vector x0 = this->ritzVector_; Vector x00 = previousVector_; - R.col(0) = update(x0, x00, betas[k]); - R.col(1) = update(R.col(0), x0, betas[k]); + R.col(0) = powerIteration(x0, x00, betas[k]); + R.col(1) = powerIteration(R.col(0), x0, betas[k]); } else { - R.col(j) = update(R.col(j - 1), R.col(j - 2), betas[k]); + R.col(j) = powerIteration(R.col(j - 1), R.col(j - 2), betas[k]); } } const Vector x = R.col(9); diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index 4807e0e6a..ee7a04429 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -46,8 +46,8 @@ class PowerMethod { size_t nrIterations_; // number of iterations - double ritzValue_; // all Ritz eigenvalues - Vector ritzVector_; // all Ritz eigenvectors + double ritzValue_; // Ritz eigenvalue + Vector ritzVector_; // Ritz eigenvector public: // Constructor @@ -55,85 +55,61 @@ class PowerMethod { const boost::optional initial = boost::none) : A_(A), dim_(A.rows()), nrIterations_(0) { Vector x0; - x0 = initial ? Vector::Random(dim_) : initial.get(); + x0 = initial ? initial.get() : Vector::Random(dim_); x0.normalize(); - // initialize Ritz eigen values + // initialize Ritz eigen value ritzValue_ = 0.0; // initialize Ritz eigen vectors ritzVector_ = Vector::Zero(dim_); - - ritzVector_.col(0) = update(x0); - perturb(); + ritzVector_ = powerIteration(x0); } // Update the vector by dot product with A_ - Vector update(const Vector &x) const { + Vector powerIteration(const Vector &x) const { Vector y = A_ * x; y.normalize(); return y; } // Update the vector by dot product with A_ - Vector update() const { return update(ritzVector_); } + Vector powerIteration() const { return powerIteration(ritzVector_); } - // Perturb the initial ritzvector - void perturb() { - // generate a 0.03*||x_0||_2 as stated in David's paper - std::mt19937 rng(42); - std::uniform_real_distribution uniform01(0.0, 1.0); - - int n = dim_; - // Vector disturb(n); - // for (int i = 0; i < n; ++i) { - // disturb(i) = uniform01(rng); - // } - Vector disturb = Vector::Random(n); - disturb.normalize(); - - Vector x0 = ritzVector_; - double magnitude = x0.norm(); - ritzVector_ = x0 + 0.03 * magnitude * disturb; - } - - // Perform power iteration on a single Ritz value - // Updates ritzValue_ - bool iterateOne(double tol) { + // After Perform power iteration on a single Ritz value, if the error is less + // than the tol then return true else false + bool converged(double tol) { const Vector x = ritzVector_; - double theta = x.transpose() * A_ * x; - // store the Ritz eigen value - ritzValue_ = theta; - - const Vector diff = A_ * x - theta * x; - double error = diff.norm(); + ritzValue_ = x.dot(A_ * x); + double error = (A_ * x - ritzValue_ * x).norm(); return error < tol; } // Return the number of iterations size_t nrIterations() const { return nrIterations_; } - // Start the iteration until the ritz error converge - int compute(int maxIterations, double tol) { + // Start the power/accelerated iteration, after updated the ritz vector, + // calculate the ritz error, repeat this operation until the ritz error converge + int compute(size_t maxIterations, double tol) { // Starting - int nrConverged = 0; + bool isConverged = false; - for (int i = 0; i < maxIterations; i++) { - nrIterations_ += 1; - ritzVector_ = update(); - nrConverged = iterateOne(tol); - if (nrConverged) break; + for (size_t i = 0; i < maxIterations; i++) { + ++nrIterations_ ; + ritzVector_ = powerIteration(); + isConverged = converged(tol); + if (isConverged) return isConverged; } - return std::min(1, nrConverged); + return isConverged; } // Return the eigenvalue - double eigenvalues() const { return ritzValue_; } + double eigenvalue() const { return ritzValue_; } // Return the eigenvector - const Vector eigenvectors() const { return ritzVector_; } + Vector eigenvector() const { return ritzVector_; } }; } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 5f509c91e..60233fc6e 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -471,14 +471,40 @@ Sparse ShonanAveraging::computeA(const Values &values) const { return Lambda - Q_; } +/* ************************************************************************* */ +template +Sparse ShonanAveraging::computeA(const Matrix &S) const { + auto Lambda = computeLambda(S); + return Lambda - Q_; +} + +/* ************************************************************************* */ +// Perturb the initial initialVector by adding a spherically-uniformly +// distributed random vector with 0.03*||initialVector||_2 magnitude to +// initialVector +Vector perturb(const Vector &initialVector) { + // generate a 0.03*||x_0||_2 as stated in David's paper + int n = initialVector.rows(); + Vector disturb = Vector::Random(n); + disturb.normalize(); + + double magnitude = initialVector.norm(); + Vector perturbedVector = initialVector + 0.03 * magnitude * disturb; + perturbedVector.normalize(); + return perturbedVector; +} + /* ************************************************************************* */ /// MINIMUM EIGENVALUE COMPUTATIONS -// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization, -// it takes in the certificate matrix A as input, the maxIterations and the -// minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default, +// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization, +// it takes in the certificate matrix A as input, the maxIterations and the +// minEigenvalueNonnegativityTolerance is set to 1000 and 10e-4 ad default, // there are two parts // in this algorithm: -// (1) +// (1) compute the maximum eigenpair (\lamda_dom, \vect{v}_dom) of A by power +// method. if \lamda_dom is less than zero, then return the eigenpair. (2) +// compute the maximum eigenpair (\theta, \vect{v}) of C = \lamda_dom * I - A by +// accelerated power method. Then return (\lamda_dom - \theta, \vect{v}). static bool PowerMinimumEigenValue( const Sparse &A, const Matrix &S, double *minEigenValue, Vector *minEigenVector = 0, size_t *numIterations = 0, @@ -486,39 +512,39 @@ static bool PowerMinimumEigenValue( double minEigenvalueNonnegativityTolerance = 10e-4) { // a. Compute dominant eigenpair of S using power method - const boost::optional initial(S.row(0)); - PowerMethod lmOperator(A, initial); + PowerMethod lmOperator(A); - const int lmConverged = lmOperator.compute( + const bool lmConverged = lmOperator.compute( maxIterations, 1e-5); // Check convergence and bail out if necessary - if (lmConverged != 1) return false; + if (!lmConverged) return false; - const double lmEigenValue = lmOperator.eigenvalues(); + const double lmEigenValue = lmOperator.eigenvalue(); 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 = lmOperator.eigenvectors(); + *minEigenVector = lmOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + const boost::optional initial = perturb(S.row(0)); AcceleratedPowerMethod minShiftedOperator(C, initial); - const int minConverged = minShiftedOperator.compute( + const bool minConverged = minShiftedOperator.compute( maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); - if (minConverged != 1) return false; + if (!minConverged) return false; - *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues(); + *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalue(); if (minEigenVector) { - *minEigenVector = minShiftedOperator.eigenvectors(); + *minEigenVector = minShiftedOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } if (numIterations) *numIterations = minShiftedOperator.nrIterations(); @@ -669,13 +695,6 @@ static bool SparseMinimumEigenValue( return true; } -/* ************************************************************************* */ -template -Sparse ShonanAveraging::computeA(const Matrix &S) const { - auto Lambda = computeLambda(S); - return Lambda - Q_; -} - /* ************************************************************************* */ template double ShonanAveraging::computeMinEigenValue(const Values &values, From f86ad955827efd5397b4f392a596d1e975f2d2a4 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Thu, 22 Oct 2020 14:19:23 -0400 Subject: [PATCH 09/22] Correct unittest input error --- .../tests/testAcceleratedPowerMethod.cpp | 37 ++++++++----------- gtsam/linear/tests/testPowerMethod.cpp | 35 +++++++++--------- 2 files changed, 33 insertions(+), 39 deletions(-) diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index a84a31a0f..474492475 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -42,26 +42,24 @@ TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { // test power iteration, beta is set to 0 Sparse A(6, 6); A.coeffRef(0, 0) = 6; - A.coeffRef(0, 0) = 5; - A.coeffRef(0, 0) = 4; - A.coeffRef(0, 0) = 3; - A.coeffRef(0, 0) = 2; - A.coeffRef(0, 0) = 1; - Vector initial = Vector6::Zero(); + A.coeffRef(1, 1) = 5; + A.coeffRef(2, 2) = 4; + A.coeffRef(3, 3) = 3; + A.coeffRef(4, 4) = 2; + A.coeffRef(5, 5) = 1; + Vector initial = Vector6::Random(); const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); - const double ev1 = 1.0; + const double ev1 = 6.0; // test accelerated power iteration AcceleratedPowerMethod apf(A, initial); - apf.compute(20, 1e-4); - EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols()); - EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows()); + apf.compute(50, 1e-4); + EXPECT_LONGS_EQUAL(6, apf.eigenvector().rows()); - Vector6 actual1 = apf.eigenvectors(); - // actual1(0) = abs (actual1(0)); - EXPECT(assert_equal(x1, actual1)); + Vector6 actual1 = apf.eigenvector(); + EXPECT(assert_equal(x1, actual1, 1e-4)); - EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-5); + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-5); } /* ************************************************************************* */ @@ -76,10 +74,7 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); - cout << L.first << endl; Eigen::EigenSolver solver(L.first); - cout << solver.eigenvalues() << endl; - cout << solver.eigenvectors() << endl; // Check that we get zero eigenvalue and "constant" eigenvector EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); @@ -95,11 +90,11 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { const auto ev1 = solver.eigenvalues()(maxIdx).real(); auto ev2 = solver.eigenvectors().col(maxIdx).real(); - Vector initial = Vector4::Zero(); + Vector initial = Vector4::Random(); AcceleratedPowerMethod apf(L.first, initial); - apf.compute(20, 1e-4); - EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalues(), 1e-8); - EXPECT(assert_equal(ev2, apf.eigenvectors(), 3e-5)); + apf.compute(50, 1e-4); + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); + EXPECT(assert_equal(-ev2, apf.eigenvector(), 3e-5)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 621286c85..58d1ca0cc 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -42,23 +42,22 @@ TEST(PowerMethod, powerIteration) { // test power iteration, beta is set to 0 Sparse A(6, 6); A.coeffRef(0, 0) = 6; - A.coeffRef(0, 0) = 5; - A.coeffRef(0, 0) = 4; - A.coeffRef(0, 0) = 3; - A.coeffRef(0, 0) = 2; - A.coeffRef(0, 0) = 1; - Vector initial = Vector6::Zero(); + A.coeffRef(1, 1) = 5; + A.coeffRef(2, 2) = 4; + A.coeffRef(3, 3) = 3; + A.coeffRef(4, 4) = 2; + A.coeffRef(5, 5) = 1; + Vector initial = Vector6::Random(); PowerMethod pf(A, initial); - pf.compute(20, 1e-4); - EXPECT_LONGS_EQUAL(1, pf.eigenvectors().cols()); - EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows()); + pf.compute(50, 1e-4); + EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows()); const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); - Vector6 actual0 = pf.eigenvectors(); - EXPECT(assert_equal(x1, actual0)); + Vector6 actual0 = pf.eigenvector(); + EXPECT(assert_equal(x1, actual0, 1e-4)); - const double ev1 = 1.0; - EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-5); + const double ev1 = 6.0; + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-5); } /* ************************************************************************* */ @@ -89,12 +88,12 @@ TEST(PowerMethod, useFactorGraph) { const auto ev1 = solver.eigenvalues()(maxIdx).real(); auto ev2 = solver.eigenvectors().col(maxIdx).real(); - Vector initial = Vector4::Zero(); + Vector initial = Vector4::Random(); PowerMethod pf(L.first, initial); - pf.compute(20, 1e-4); - EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-8); - // auto actual2 = pf.eigenvectors(); - // EXPECT(assert_equal(ev2, actual2, 3e-5)); + pf.compute(50, 1e-4); + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); + auto actual2 = pf.eigenvector(); + EXPECT(assert_equal(-ev2, actual2, 3e-5)); } /* ************************************************************************* */ From 32bf81efeacd6057f9aa06ee401bc8176e8a0d3f Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Thu, 22 Oct 2020 15:51:36 -0400 Subject: [PATCH 10/22] Added more detailed documentation --- gtsam/linear/AcceleratedPowerMethod.h | 3 +-- gtsam/linear/PowerMethod.h | 10 ++++---- .../tests/testAcceleratedPowerMethod.cpp | 24 ++++++++++--------- gtsam/linear/tests/testPowerMethod.cpp | 16 ++++++------- gtsam/sfm/ShonanAveraging.cpp | 3 +++ 5 files changed, 30 insertions(+), 26 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0ad87626b..2e54775d9 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -80,8 +80,7 @@ class AcceleratedPowerMethod : public PowerMethod { } else { beta_ = initialBeta; } - - } + } // Update the ritzVector with beta and previous two ritzVector Vector powerIteration(const Vector &x1, const Vector &x0, diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index ee7a04429..acb583296 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -89,14 +89,16 @@ class PowerMethod { // Return the number of iterations size_t nrIterations() const { return nrIterations_; } - // Start the power/accelerated iteration, after updated the ritz vector, - // calculate the ritz error, repeat this operation until the ritz error converge - int compute(size_t maxIterations, double tol) { + // Start the power/accelerated iteration, after performing the + // power/accelerated power iteration, calculate the ritz error, repeat this + // operation until the ritz error converge. If converged return true, else + // false. + bool compute(size_t maxIterations, double tol) { // Starting bool isConverged = false; for (size_t i = 0; i < maxIterations; i++) { - ++nrIterations_ ; + ++nrIterations_; ritzVector_ = powerIteration(); isConverged = converged(tol); if (isConverged) return isConverged; diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index 474492475..b72556e0a 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -18,18 +18,16 @@ * @brief Check eigenvalue and eigenvector computed by accelerated power method */ +#include #include #include #include -#include #include - -#include +#include #include #include #include - #include #include @@ -70,7 +68,7 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { for (size_t j = 0; j < 3; j++) { fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); } - fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); @@ -79,22 +77,26 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { // Check that we get zero eigenvalue and "constant" eigenvector EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); auto v0 = solver.eigenvectors().col(0); - for (size_t j = 0; j < 3; j++) - EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); size_t maxIdx = 0; - for (auto i =0; i= solver.eigenvalues()(maxIdx).real()) maxIdx = i; + for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { + if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) + maxIdx = i; } // Store the max eigenvalue and its according eigenvector const auto ev1 = solver.eigenvalues()(maxIdx).real(); auto ev2 = solver.eigenvectors().col(maxIdx).real(); - Vector initial = Vector4::Random(); + Vector disturb = Vector4::Random(); + disturb.normalize(); + Vector initial = L.first.row(0); + double magnitude = initial.norm(); + initial += 0.03 * magnitude * disturb; AcceleratedPowerMethod apf(L.first, initial); apf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); - EXPECT(assert_equal(-ev2, apf.eigenvector(), 3e-5)); + EXPECT(assert_equal(ev2, apf.eigenvector(), 3e-5)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 58d1ca0cc..7f6d1efa7 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -18,18 +18,16 @@ * @brief Check eigenvalue and eigenvector computed by power method */ +#include #include #include #include #include #include -#include - #include #include #include - #include #include @@ -68,7 +66,7 @@ TEST(PowerMethod, useFactorGraph) { for (size_t j = 0; j < 3; j++) { fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); } - fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); @@ -77,12 +75,12 @@ TEST(PowerMethod, useFactorGraph) { // Check that we get zero eigenvalue and "constant" eigenvector EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); auto v0 = solver.eigenvectors().col(0); - for (size_t j = 0; j < 3; j++) - EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); size_t maxIdx = 0; - for (auto i =0; i= solver.eigenvalues()(maxIdx).real()) maxIdx = i; + for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { + if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) + maxIdx = i; } // Store the max eigenvalue and its according eigenvector const auto ev1 = solver.eigenvalues()(maxIdx).real(); @@ -92,7 +90,7 @@ TEST(PowerMethod, useFactorGraph) { PowerMethod pf(L.first, initial); pf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); - auto actual2 = pf.eigenvector(); + auto actual2 = pf.eigenvector(); EXPECT(assert_equal(-ev2, actual2, 3e-5)); } diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 60233fc6e..50b38f75b 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -482,6 +482,9 @@ Sparse ShonanAveraging::computeA(const Matrix &S) const { // Perturb the initial initialVector by adding a spherically-uniformly // distributed random vector with 0.03*||initialVector||_2 magnitude to // initialVector +// ref : Part III. C, Rosen, D. and Carlone, L., 2017, September. Computational +// enhancements for certifiably correct SLAM. In Proceedings of the +// International Conference on Intelligent Robots and Systems. Vector perturb(const Vector &initialVector) { // generate a 0.03*||x_0||_2 as stated in David's paper int n = initialVector.rows(); From 56300ca23c292f2e1142600fd3de8c0b7dbedc8d Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Thu, 22 Oct 2020 18:05:09 -0400 Subject: [PATCH 11/22] Fixed not solve errors and detailed documentation --- gtsam/linear/AcceleratedPowerMethod.h | 33 +++++++++---------- gtsam/linear/PowerMethod.h | 6 ++-- .../tests/testAcceleratedPowerMethod.cpp | 2 +- gtsam/sfm/ShonanAveraging.cpp | 28 ++++++++-------- 4 files changed, 33 insertions(+), 36 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 2e54775d9..a5148e1d4 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -63,16 +63,10 @@ class AcceleratedPowerMethod : public PowerMethod { const Operator &A, const boost::optional initial = boost::none, const double initialBeta = 0.0) : PowerMethod(A, initial) { - Vector x0; - // initialize ritz vector - x0 = initial ? initial.get() : Vector::Random(this->dim_); - Vector x00 = Vector::Random(this->dim_); - x0.normalize(); - x00.normalize(); - // initialize Ritz eigen vector and previous vector - previousVector_ = powerIteration(x0, x00, beta_); - this->ritzVector_ = powerIteration(previousVector_, x0, beta_); + this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); + this->ritzVector_.normalize(); + previousVector_ = Vector::Zero(this->dim_); // initialize beta_ if (!initialBeta) { @@ -80,9 +74,10 @@ class AcceleratedPowerMethod : public PowerMethod { } else { beta_ = initialBeta; } - } + } - // Update the ritzVector with beta and previous two ritzVector + // Update the ritzVector with beta and previous two ritzVector, and return + // x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - \beta * x_{k-1} || Vector powerIteration(const Vector &x1, const Vector &x0, const double beta) const { Vector y = this->A_ * x1 - beta * x0; @@ -90,7 +85,8 @@ class AcceleratedPowerMethod : public PowerMethod { return y; } - // Update the ritzVector with beta and previous two ritzVector + // Update the ritzVector with beta and previous two ritzVector, and return + // x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - \beta * x_{k-1} || Vector powerIteration() const { Vector y = powerIteration(this->ritzVector_, previousVector_, beta_); previousVector_ = this->ritzVector_; @@ -101,8 +97,8 @@ class AcceleratedPowerMethod : public PowerMethod { void estimateBeta() { // set beta Vector init_resid = this->ritzVector_; - const double up = init_resid.transpose() * this->A_ * init_resid; - const double down = init_resid.transpose().dot(init_resid); + const double up = init_resid.dot( this->A_ * init_resid ); + const double down = init_resid.dot(init_resid); const double mu = up / down; double maxBeta = mu * mu / 4; size_t maxIndex; @@ -111,11 +107,12 @@ class AcceleratedPowerMethod : public PowerMethod { Matrix R = Matrix::Zero(this->dim_, 10); for (size_t i = 0; i < 10; i++) { + Vector x0 = Vector::Random(this->dim_); + x0.normalize(); + Vector x00 = Vector::Zero(this->dim_); for (size_t k = 0; k < betas.size(); ++k) { for (size_t j = 1; j < 10; j++) { if (j < 2) { - Vector x0 = this->ritzVector_; - Vector x00 = previousVector_; R.col(0) = powerIteration(x0, x00, betas[k]); R.col(1) = powerIteration(R.col(0), x0, betas[k]); } else { @@ -123,8 +120,8 @@ class AcceleratedPowerMethod : public PowerMethod { } } const Vector x = R.col(9); - const double up = x.transpose() * this->A_ * x; - const double down = x.transpose().dot(x); + const double up = x.dot(this->A_ * x); + const double down = x.dot(x); const double mu = up / down; if (mu * mu / 4 > maxBeta) { maxIndex = k; diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index acb583296..8a577aa92 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -61,19 +61,19 @@ class PowerMethod { // initialize Ritz eigen value ritzValue_ = 0.0; - // initialize Ritz eigen vectors + // initialize Ritz eigen vector ritzVector_ = Vector::Zero(dim_); ritzVector_ = powerIteration(x0); } - // Update the vector by dot product with A_ + // Update the vector by dot product with A_, and return A * x / || A * x || Vector powerIteration(const Vector &x) const { Vector y = A_ * x; y.normalize(); return y; } - // Update the vector by dot product with A_ + // Update the vector by dot product with A_, and return A * x / || A * x || Vector powerIteration() const { return powerIteration(ritzVector_); } // After Perform power iteration on a single Ritz value, if the error is less diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index b72556e0a..6179d6ca6 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -96,7 +96,7 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { AcceleratedPowerMethod apf(L.first, initial); apf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); - EXPECT(assert_equal(ev2, apf.eigenvector(), 3e-5)); + EXPECT(assert_equal(ev2, apf.eigenvector(), 4e-5)); } /* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 50b38f75b..8d2bf4653 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -515,42 +515,42 @@ static bool PowerMinimumEigenValue( double minEigenvalueNonnegativityTolerance = 10e-4) { // a. Compute dominant eigenpair of S using power method - PowerMethod lmOperator(A); + PowerMethod pmOperator(A); - const bool lmConverged = lmOperator.compute( + const bool pmConverged = pmOperator.compute( maxIterations, 1e-5); // Check convergence and bail out if necessary - if (!lmConverged) return false; + if (!pmConverged) return false; - const double lmEigenValue = lmOperator.eigenvalue(); + const double pmEigenValue = pmOperator.eigenvalue(); - if (lmEigenValue < 0) { + if (pmEigenValue < 0) { // The largest-magnitude eigenvalue is negative, and therefore also the // minimum eigenvalue, so just return this solution - *minEigenValue = lmEigenValue; + *minEigenValue = pmEigenValue; if (minEigenVector) { - *minEigenVector = lmOperator.eigenvector(); + *minEigenVector = pmOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } - const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; const boost::optional initial = perturb(S.row(0)); - AcceleratedPowerMethod minShiftedOperator(C, initial); + AcceleratedPowerMethod apmShiftedOperator(C, initial); - const bool minConverged = minShiftedOperator.compute( - maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); + const bool minConverged = apmShiftedOperator.compute( + maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue); if (!minConverged) return false; - *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalue(); + *minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue(); if (minEigenVector) { - *minEigenVector = minShiftedOperator.eigenvector(); + *minEigenVector = apmShiftedOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } - if (numIterations) *numIterations = minShiftedOperator.nrIterations(); + if (numIterations) *numIterations = apmShiftedOperator.nrIterations(); return true; } From cf813c5a64b213ffd46a7ae7a6add1a2f342b80c Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 26 Oct 2020 15:40:49 -0400 Subject: [PATCH 12/22] Revised as David's second review --- gtsam/linear/AcceleratedPowerMethod.h | 78 +++++++++++++++++++-------- gtsam/linear/PowerMethod.h | 23 ++++---- gtsam/sfm/ShonanAveraging.cpp | 10 ++-- 3 files changed, 75 insertions(+), 36 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index a5148e1d4..eea007353 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -61,7 +61,7 @@ class AcceleratedPowerMethod : public PowerMethod { // vector as ritzVector explicit AcceleratedPowerMethod( const Operator &A, const boost::optional initial = boost::none, - const double initialBeta = 0.0) + double initialBeta = 0.0) : PowerMethod(A, initial) { // initialize Ritz eigen vector and previous vector this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); @@ -76,61 +76,97 @@ class AcceleratedPowerMethod : public PowerMethod { } } - // Update the ritzVector with beta and previous two ritzVector, and return - // x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - \beta * x_{k-1} || - Vector powerIteration(const Vector &x1, const Vector &x0, + // Run accelerated power iteration on the ritzVector with beta and previous + // two ritzVector, and return x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k + // - \beta * x_{k-1} || + Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0, const double beta) const { Vector y = this->A_ * x1 - beta * x0; y.normalize(); return y; } - // Update the ritzVector with beta and previous two ritzVector, and return - // x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - \beta * x_{k-1} || - Vector powerIteration() const { - Vector y = powerIteration(this->ritzVector_, previousVector_, beta_); - previousVector_ = this->ritzVector_; + // Run accelerated power iteration on the ritzVector with beta and previous + // two ritzVector, and return x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k + // - \beta * x_{k-1} || + Vector acceleratedPowerIteration () const { + Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_); return y; } // Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) void estimateBeta() { // set beta - Vector init_resid = this->ritzVector_; - const double up = init_resid.dot( this->A_ * init_resid ); - const double down = init_resid.dot(init_resid); + Vector initVector = this->ritzVector_; + const double up = initVector.dot( this->A_ * initVector ); + const double down = initVector.dot(initVector); const double mu = up / down; double maxBeta = mu * mu / 4; size_t maxIndex; - std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, - 1.01 * maxBeta, 1.5 * maxBeta}; + std::vector betas; Matrix R = Matrix::Zero(this->dim_, 10); - for (size_t i = 0; i < 10; i++) { - Vector x0 = Vector::Random(this->dim_); - x0.normalize(); - Vector x00 = Vector::Zero(this->dim_); + size_t T = 10; + // run T times of iteration to find the beta that has the largest Rayleigh quotient + for (size_t t = 0; t < T; t++) { + // after each t iteration, reset the betas with the current maxBeta + betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta, + 1.5 * maxBeta}; + // iterate through every beta value for (size_t k = 0; k < betas.size(); ++k) { + // initialize x0 and x00 in each iteration of each beta + Vector x0 = initVector; + Vector x00 = Vector::Zero(this->dim_); + // run 10 steps of accelerated power iteration with this beta for (size_t j = 1; j < 10; j++) { if (j < 2) { - R.col(0) = powerIteration(x0, x00, betas[k]); - R.col(1) = powerIteration(R.col(0), x0, betas[k]); + R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]); + R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]); } else { - R.col(j) = powerIteration(R.col(j - 1), R.col(j - 2), betas[k]); + R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2), betas[k]); } } + // compute the Rayleigh quotient for the randomly sampled vector after + // 10 steps of power accelerated iteration const Vector x = R.col(9); const double up = x.dot(this->A_ * x); const double down = x.dot(x); const double mu = up / down; + // store the momentum with largest Rayleigh quotient and its according index of beta_ if (mu * mu / 4 > maxBeta) { + // save the max beta index maxIndex = k; maxBeta = mu * mu / 4; } } } + // set beta_ to momentum with largest Rayleigh quotient beta_ = betas[maxIndex]; } + + // Start the accelerated iteration, after performing the + // accelerated iteration, calculate the ritz error, repeat this + // operation until the ritz error converge. If converged return true, else + // false. + bool compute(size_t maxIterations, double tol) { + // Starting + bool isConverged = false; + + for (size_t i = 0; i < maxIterations; i++) { + ++(this->nrIterations_); + Vector tmp = this->ritzVector_; + // update the ritzVector after accelerated power iteration + this->ritzVector_ = acceleratedPowerIteration(); + // update the previousVector with ritzVector + previousVector_ = tmp; + // update the ritzValue + this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_); + isConverged = this->converged(tol); + if (isConverged) return isConverged; + } + + return isConverged; + } }; } // namespace gtsam diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index 8a577aa92..ae1d97cc7 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -66,23 +66,24 @@ class PowerMethod { ritzVector_ = powerIteration(x0); } - // Update the vector by dot product with A_, and return A * x / || A * x || + // Run power iteration on the vector, and return A * x / || A * x || Vector powerIteration(const Vector &x) const { Vector y = A_ * x; y.normalize(); return y; } - // Update the vector by dot product with A_, and return A * x / || A * x || + // Run power iteration on the vector, and return A * x / || A * x || Vector powerIteration() const { return powerIteration(ritzVector_); } - // After Perform power iteration on a single Ritz value, if the error is less - // than the tol then return true else false - bool converged(double tol) { + // After Perform power iteration on a single Ritz value, check if the Ritz + // residual for the current Ritz pair is less than the required convergence + // tol, return true if yes, else false + bool converged(double tol) const { const Vector x = ritzVector_; // store the Ritz eigen value - ritzValue_ = x.dot(A_ * x); - double error = (A_ * x - ritzValue_ * x).norm(); + const double ritzValue = x.dot(A_ * x); + const double error = (A_ * x - ritzValue * x).norm(); return error < tol; } @@ -90,18 +91,20 @@ class PowerMethod { size_t nrIterations() const { return nrIterations_; } // Start the power/accelerated iteration, after performing the - // power/accelerated power iteration, calculate the ritz error, repeat this + // power/accelerated iteration, calculate the ritz error, repeat this // operation until the ritz error converge. If converged return true, else // false. bool compute(size_t maxIterations, double tol) { // Starting bool isConverged = false; - for (size_t i = 0; i < maxIterations; i++) { + for (size_t i = 0; i < maxIterations && !isConverged; i++) { ++nrIterations_; + // update the ritzVector after power iteration ritzVector_ = powerIteration(); + // update the ritzValue + ritzValue_ = ritzVector_.dot(A_ * ritzVector_); isConverged = converged(tol); - if (isConverged) return isConverged; } return isConverged; diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 8d2bf4653..fab57c828 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -509,7 +509,7 @@ Vector perturb(const Vector &initialVector) { // compute the maximum eigenpair (\theta, \vect{v}) of C = \lamda_dom * I - A by // accelerated power method. Then return (\lamda_dom - \theta, \vect{v}). static bool PowerMinimumEigenValue( - const Sparse &A, const Matrix &S, double *minEigenValue, + const Sparse &A, const Matrix &S, double &minEigenValue, Vector *minEigenVector = 0, size_t *numIterations = 0, size_t maxIterations = 1000, double minEigenvalueNonnegativityTolerance = 10e-4) { @@ -528,7 +528,7 @@ static bool PowerMinimumEigenValue( if (pmEigenValue < 0) { // The largest-magnitude eigenvalue is negative, and therefore also the // minimum eigenvalue, so just return this solution - *minEigenValue = pmEigenValue; + minEigenValue = pmEigenValue; if (minEigenVector) { *minEigenVector = pmOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector @@ -545,7 +545,7 @@ static bool PowerMinimumEigenValue( if (!minConverged) return false; - *minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue(); + minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue(); if (minEigenVector) { *minEigenVector = apmShiftedOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector @@ -723,8 +723,8 @@ double ShonanAveraging::computeMinEigenValueAP(const Values &values, const Matrix S = StiefelElementMatrix(values); auto A = computeA(S); - double minEigenValue; - bool success = PowerMinimumEigenValue(A, S, &minEigenValue, minEigenVector); + double minEigenValue = 0; + bool success = PowerMinimumEigenValue(A, S, minEigenValue, minEigenVector); if (!success) { throw std::runtime_error( "PowerMinimumEigenValue failed to compute minimum eigenvalue."); From 4ee4014d7a345fc410461c1d2855ef296b8820ba Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 26 Oct 2020 15:41:01 -0400 Subject: [PATCH 13/22] Refined unittest --- .../tests/testAcceleratedPowerMethod.cpp | 23 +++++++++++++------ gtsam/linear/tests/testPowerMethod.cpp | 15 +++++++----- 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index 6179d6ca6..228ce157c 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -45,17 +45,19 @@ TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { A.coeffRef(3, 3) = 3; A.coeffRef(4, 4) = 2; A.coeffRef(5, 5) = 1; - Vector initial = Vector6::Random(); - const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359, + 0.2465342).finished(); const double ev1 = 6.0; // test accelerated power iteration AcceleratedPowerMethod apf(A, initial); - apf.compute(50, 1e-4); + apf.compute(100, 1e-5); EXPECT_LONGS_EQUAL(6, apf.eigenvector().rows()); Vector6 actual1 = apf.eigenvector(); - EXPECT(assert_equal(x1, actual1, 1e-4)); + const double ritzValue = actual1.dot(A * actual1); + const double ritzResidual = (A * actual1 - ritzValue * actual1).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-5); } @@ -79,6 +81,7 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { auto v0 = solver.eigenvectors().col(0); for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + // find the index of the max eigenvalue size_t maxIdx = 0; for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) @@ -86,7 +89,6 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { } // Store the max eigenvalue and its according eigenvector const auto ev1 = solver.eigenvalues()(maxIdx).real(); - auto ev2 = solver.eigenvectors().col(maxIdx).real(); Vector disturb = Vector4::Random(); disturb.normalize(); @@ -94,9 +96,16 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { double magnitude = initial.norm(); initial += 0.03 * magnitude * disturb; AcceleratedPowerMethod apf(L.first, initial); - apf.compute(50, 1e-4); + apf.compute(100, 1e-5); + // Check if the eigenvalue is the maximum eigen value EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); - EXPECT(assert_equal(ev2, apf.eigenvector(), 4e-5)); + + // Check if the according ritz residual converged to the threshold + Vector actual1 = apf.eigenvector(); + const double ritzValue = actual1.dot(L.first * actual1); + const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); + // Check } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 7f6d1efa7..4c96c5bca 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -45,14 +45,16 @@ TEST(PowerMethod, powerIteration) { A.coeffRef(3, 3) = 3; A.coeffRef(4, 4) = 2; A.coeffRef(5, 5) = 1; - Vector initial = Vector6::Random(); + Vector initial = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359, + 0.2465342).finished(); PowerMethod pf(A, initial); - pf.compute(50, 1e-4); + pf.compute(100, 1e-5); EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows()); - const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); - Vector6 actual0 = pf.eigenvector(); - EXPECT(assert_equal(x1, actual0, 1e-4)); + Vector6 actual1 = pf.eigenvector(); + const double ritzValue = actual1.dot(A * actual1); + const double ritzResidual = (A * actual1 - ritzValue * actual1).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); const double ev1 = 6.0; EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-5); @@ -77,6 +79,7 @@ TEST(PowerMethod, useFactorGraph) { auto v0 = solver.eigenvectors().col(0); for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); + // find the index of the max eigenvalue size_t maxIdx = 0; for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) @@ -91,7 +94,7 @@ TEST(PowerMethod, useFactorGraph) { pf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); auto actual2 = pf.eigenvector(); - EXPECT(assert_equal(-ev2, actual2, 3e-5)); + EXPECT(assert_equal(ev2, actual2, 3e-5)); } /* ************************************************************************* */ From f604a9784de84a3c9699be737f295e77976b9a5b Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sun, 1 Nov 2020 20:45:59 -0500 Subject: [PATCH 14/22] Delete forcing compare eigenvector in unittest --- gtsam/linear/tests/testAcceleratedPowerMethod.cpp | 6 ------ gtsam/linear/tests/testPowerMethod.cpp | 5 ----- 2 files changed, 11 deletions(-) diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index 228ce157c..dd593e7d3 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -76,11 +76,6 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { auto L = fg.hessian(); Eigen::EigenSolver solver(L.first); - // Check that we get zero eigenvalue and "constant" eigenvector - EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); - auto v0 = solver.eigenvectors().col(0); - for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); - // find the index of the max eigenvalue size_t maxIdx = 0; for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { @@ -105,7 +100,6 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { const double ritzValue = actual1.dot(L.first * actual1); const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm(); EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); - // Check } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 4c96c5bca..ccac4556c 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -74,11 +74,6 @@ TEST(PowerMethod, useFactorGraph) { auto L = fg.hessian(); Eigen::EigenSolver solver(L.first); - // Check that we get zero eigenvalue and "constant" eigenvector - EXPECT_DOUBLES_EQUAL(0.0, solver.eigenvalues()[0].real(), 1e-9); - auto v0 = solver.eigenvectors().col(0); - for (size_t j = 0; j < 3; j++) EXPECT_DOUBLES_EQUAL(-0.5, v0[j].real(), 1e-9); - // find the index of the max eigenvalue size_t maxIdx = 0; for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { From 70cecb3a61d14f1b1f960f888b1a93f42a43eed6 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sun, 1 Nov 2020 20:46:42 -0500 Subject: [PATCH 15/22] Revised documentation --- gtsam/linear/AcceleratedPowerMethod.h | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index eea007353..0c544f05c 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -77,8 +77,8 @@ class AcceleratedPowerMethod : public PowerMethod { } // Run accelerated power iteration on the ritzVector with beta and previous - // two ritzVector, and return x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - // - \beta * x_{k-1} || + // two ritzVector, and return y = (A * x0 - \beta * x00) / || A * x0 + // - \beta * x00 || Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0, const double beta) const { Vector y = this->A_ * x1 - beta * x0; @@ -87,8 +87,8 @@ class AcceleratedPowerMethod : public PowerMethod { } // Run accelerated power iteration on the ritzVector with beta and previous - // two ritzVector, and return x_{k+1} = A * x_k - \beta * x_{k-1} / || A * x_k - // - \beta * x_{k-1} || + // two ritzVector, and return y = (A * x0 - \beta * x00) / || A * x0 + // - \beta * x00 || Vector acceleratedPowerIteration () const { Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_); return y; @@ -96,7 +96,7 @@ class AcceleratedPowerMethod : public PowerMethod { // Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) void estimateBeta() { - // set beta + // set initial estimation of maxBeta Vector initVector = this->ritzVector_; const double up = initVector.dot( this->A_ * initVector ); const double down = initVector.dot(initVector); @@ -152,7 +152,7 @@ class AcceleratedPowerMethod : public PowerMethod { // Starting bool isConverged = false; - for (size_t i = 0; i < maxIterations; i++) { + for (size_t i = 0; i < maxIterations && !isConverged; i++) { ++(this->nrIterations_); Vector tmp = this->ritzVector_; // update the ritzVector after accelerated power iteration @@ -162,7 +162,6 @@ class AcceleratedPowerMethod : public PowerMethod { // update the ritzValue this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_); isConverged = this->converged(tol); - if (isConverged) return isConverged; } return isConverged; From abfc98e13d738c4ac4e97147e52a2cacecd7347c Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 2 Nov 2020 15:39:39 -0500 Subject: [PATCH 16/22] Fixed forcing comparing eigenvector. --- gtsam/linear/tests/testPowerMethod.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index ccac4556c..2e0f2152b 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -82,14 +82,15 @@ TEST(PowerMethod, useFactorGraph) { } // Store the max eigenvalue and its according eigenvector const auto ev1 = solver.eigenvalues()(maxIdx).real(); - auto ev2 = solver.eigenvectors().col(maxIdx).real(); Vector initial = Vector4::Random(); PowerMethod pf(L.first, initial); - pf.compute(50, 1e-4); + pf.compute(100, 1e-5); EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); auto actual2 = pf.eigenvector(); - EXPECT(assert_equal(ev2, actual2, 3e-5)); + const double ritzValue = actual2.dot(L.first * actual2); + const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); } /* ************************************************************************* */ From 6d9f95d32eff60b4635bc665e94fab081f4c66db Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Sun, 15 Nov 2020 22:49:36 -0500 Subject: [PATCH 17/22] Fixed doxygen --- gtsam/linear/AcceleratedPowerMethod.h | 89 ++++++++++++++------------- gtsam/linear/PowerMethod.h | 70 +++++++++++++++------ 2 files changed, 96 insertions(+), 63 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0c544f05c..0699f237e 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -19,46 +19,44 @@ #pragma once -// #include -// #include #include namespace gtsam { using Sparse = Eigen::SparseMatrix; -/* ************************************************************************* */ -/// MINIMUM EIGENVALUE COMPUTATIONS - -// Template argument Operator just needs multiplication operator +/** + * \brief Compute maximum Eigenpair with accelerated power method + * + * References : + * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * enhancements for certifiably correct SLAM. In Proceedings of the + * International Conference on Intelligent Robots and Systems. + * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. + * 58–67 + * + * It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta * + * x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the + * Ritz vector + * + * Template argument Operator just needs multiplication operator + * + */ template class AcceleratedPowerMethod : public PowerMethod { - /** - * \brief Compute maximum Eigenpair with accelerated power method - * - * References : - * 1) Rosen, D. and Carlone, L., 2017, September. Computational - * enhancements for certifiably correct SLAM. In Proceedings of the - * International Conference on Intelligent Robots and Systems. - * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, - * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv - * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated - * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. - * 58–67 - * - * It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta * - * x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the - * Ritz vector - * - */ double beta_ = 0; // a Polyak momentum term Vector previousVector_; // store previous vector public: - // Constructor from aim matrix A (given as Matrix or Sparse), optional intial - // vector as ritzVector + /** + * Constructor from aim matrix A (given as Matrix or Sparse), optional intial + * vector as ritzVector + */ explicit AcceleratedPowerMethod( const Operator &A, const boost::optional initial = boost::none, double initialBeta = 0.0) @@ -70,15 +68,16 @@ class AcceleratedPowerMethod : public PowerMethod { // initialize beta_ if (!initialBeta) { - estimateBeta(); - } else { + beta_ = estimateBeta(); + } else beta_ = initialBeta; - } } - // Run accelerated power iteration on the ritzVector with beta and previous - // two ritzVector, and return y = (A * x0 - \beta * x00) / || A * x0 - // - \beta * x00 || + /** + * Run accelerated power iteration to get ritzVector with beta and previous + * two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0 + * - \beta * x00 || + */ Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0, const double beta) const { Vector y = this->A_ * x1 - beta * x0; @@ -86,16 +85,18 @@ class AcceleratedPowerMethod : public PowerMethod { return y; } - // Run accelerated power iteration on the ritzVector with beta and previous - // two ritzVector, and return y = (A * x0 - \beta * x00) / || A * x0 - // - \beta * x00 || + /** + * Run accelerated power iteration to get ritzVector with beta and previous + * two ritzVector x0 and x00, and return y = (A * x0 - \beta * x00) / || A * x0 + * - \beta * x00 || + */ Vector acceleratedPowerIteration () const { Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_); return y; } - // Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) - void estimateBeta() { + /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) + double estimateBeta() const { // set initial estimation of maxBeta Vector initVector = this->ritzVector_; const double up = initVector.dot( this->A_ * initVector ); @@ -106,7 +107,7 @@ class AcceleratedPowerMethod : public PowerMethod { std::vector betas; Matrix R = Matrix::Zero(this->dim_, 10); - size_t T = 10; + const size_t T = 10; // run T times of iteration to find the beta that has the largest Rayleigh quotient for (size_t t = 0; t < T; t++) { // after each t iteration, reset the betas with the current maxBeta @@ -141,13 +142,15 @@ class AcceleratedPowerMethod : public PowerMethod { } } // set beta_ to momentum with largest Rayleigh quotient - beta_ = betas[maxIndex]; + return betas[maxIndex]; } - // Start the accelerated iteration, after performing the - // accelerated iteration, calculate the ritz error, repeat this - // operation until the ritz error converge. If converged return true, else - // false. + /** + * Start the accelerated iteration, after performing the + * accelerated iteration, calculate the ritz error, repeat this + * operation until the ritz error converge. If converged return true, else + * false. + */ bool compute(size_t maxIterations, double tol) { // Starting bool isConverged = false; diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index ae1d97cc7..a209c5779 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -31,15 +31,33 @@ namespace gtsam { using Sparse = Eigen::SparseMatrix; -/* ************************************************************************* */ -/// MINIMUM EIGENVALUE COMPUTATIONS - -// Template argument Operator just needs multiplication operator +/** + * \brief Compute maximum Eigenpair with power method + * + * References : + * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * enhancements for certifiably correct SLAM. In Proceedings of the + * International Conference on Intelligent Robots and Systems. + * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. + * 58–67 + * + * It performs the following iteration: \f$ x_{k+1} = A * x_k \f$ + * where A is the aim matrix we want to get eigenpair of, x is the + * Ritz vector + * + * Template argument Operator just needs multiplication operator + * + */ template class PowerMethod { protected: - // Const reference to an externally-held matrix whose minimum-eigenvalue we - // want to compute + /** + * Const reference to an externally-held matrix whose minimum-eigenvalue we + * want to compute + */ const Operator &A_; const int dim_; // dimension of Matrix A @@ -50,7 +68,10 @@ class PowerMethod { Vector ritzVector_; // Ritz eigenvector public: - // Constructor + /// @name Standard Constructors + /// @{ + + /// Construct from the aim matrix and intial ritz vector explicit PowerMethod(const Operator &A, const boost::optional initial = boost::none) : A_(A), dim_(A.rows()), nrIterations_(0) { @@ -62,23 +83,30 @@ class PowerMethod { ritzValue_ = 0.0; // initialize Ritz eigen vector - ritzVector_ = Vector::Zero(dim_); ritzVector_ = powerIteration(x0); } - // Run power iteration on the vector, and return A * x / || A * x || + /** + * Run power iteration to get ritzVector with previous ritzVector x, and + * return A * x / || A * x || + */ Vector powerIteration(const Vector &x) const { Vector y = A_ * x; y.normalize(); return y; } - // Run power iteration on the vector, and return A * x / || A * x || + /** + * Run power iteration to get ritzVector with previous ritzVector x, and + * return A * x / || A * x || + */ Vector powerIteration() const { return powerIteration(ritzVector_); } - // After Perform power iteration on a single Ritz value, check if the Ritz - // residual for the current Ritz pair is less than the required convergence - // tol, return true if yes, else false + /** + * After Perform power iteration on a single Ritz value, check if the Ritz + * residual for the current Ritz pair is less than the required convergence + * tol, return true if yes, else false + */ bool converged(double tol) const { const Vector x = ritzVector_; // store the Ritz eigen value @@ -87,13 +115,15 @@ class PowerMethod { return error < tol; } - // Return the number of iterations + /// Return the number of iterations size_t nrIterations() const { return nrIterations_; } - // Start the power/accelerated iteration, after performing the - // power/accelerated iteration, calculate the ritz error, repeat this - // operation until the ritz error converge. If converged return true, else - // false. + /** + * Start the power/accelerated iteration, after performing the + * power/accelerated iteration, calculate the ritz error, repeat this + * operation until the ritz error converge. If converged return true, else + * false. + */ bool compute(size_t maxIterations, double tol) { // Starting bool isConverged = false; @@ -110,10 +140,10 @@ class PowerMethod { return isConverged; } - // Return the eigenvalue + /// Return the eigenvalue double eigenvalue() const { return ritzValue_; } - // Return the eigenvector + /// Return the eigenvector Vector eigenvector() const { return ritzVector_; } }; From 844cbead2b4d6e429af9004c1e13262a0ea8faa2 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Mon, 30 Nov 2020 18:52:33 -0500 Subject: [PATCH 18/22] Added dense matrix test case in power/acc --- .../tests/testAcceleratedPowerMethod.cpp | 50 ++++++++++++++++++- gtsam/linear/tests/testPowerMethod.cpp | 43 +++++++++++++++- 2 files changed, 91 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index dd593e7d3..c7c8e8a55 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -63,7 +63,7 @@ TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { } /* ************************************************************************* */ -TEST(AcceleratedPowerMethod, useFactorGraph) { +TEST(AcceleratedPowerMethod, useFactorGraphSparse) { // Let's make a scalar synchronization graph with 4 nodes GaussianFactorGraph fg; auto model = noiseModel::Unit::Create(1); @@ -102,6 +102,54 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); } +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, useFactorGraphDense) { + // Let's make a scalar synchronization graph with 10 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + // Each node has an edge with all the others + for (size_t j = 0; j < 10; j++) { + fg.add(X(j), -I_1x1, X((j + 1)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 2)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 3)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 4)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 5)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 6)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 7)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 8)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 9)%10 ), I_1x1, Vector1::Zero(), model); + } + + // Get eigenvalues and eigenvectors with Eigen + auto L = fg.hessian(); + Eigen::EigenSolver solver(L.first); + + // find the index of the max eigenvalue + size_t maxIdx = 0; + for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { + if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) + maxIdx = i; + } + // Store the max eigenvalue and its according eigenvector + const auto ev1 = solver.eigenvalues()(maxIdx).real(); + + Vector disturb = Vector10::Random(); + disturb.normalize(); + Vector initial = L.first.row(0); + double magnitude = initial.norm(); + initial += 0.03 * magnitude * disturb; + AcceleratedPowerMethod apf(L.first, initial); + apf.compute(100, 1e-5); + // Check if the eigenvalue is the maximum eigen value + EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); + + // Check if the according ritz residual converged to the threshold + Vector actual1 = apf.eigenvector(); + const double ritzValue = actual1.dot(L.first * actual1); + const double ritzResidual = (L.first * actual1 - ritzValue * actual1).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 2e0f2152b..7adfd0aa5 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -61,7 +61,7 @@ TEST(PowerMethod, powerIteration) { } /* ************************************************************************* */ -TEST(PowerMethod, useFactorGraph) { +TEST(PowerMethod, useFactorGraphSparse) { // Let's make a scalar synchronization graph with 4 nodes GaussianFactorGraph fg; auto model = noiseModel::Unit::Create(1); @@ -93,6 +93,47 @@ TEST(PowerMethod, useFactorGraph) { EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); } +/* ************************************************************************* */ +TEST(PowerMethod, useFactorGraphDense) { + // Let's make a scalar synchronization graph with 10 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + // Each node has an edge with all the others + for (size_t j = 0; j < 10; j++) { + fg.add(X(j), -I_1x1, X((j + 1)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 2)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 3)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 4)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 5)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 6)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 7)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 8)%10 ), I_1x1, Vector1::Zero(), model); + fg.add(X(j), -I_1x1, X((j + 9)%10 ), I_1x1, Vector1::Zero(), model); + } + + // Get eigenvalues and eigenvectors with Eigen + auto L = fg.hessian(); + Eigen::EigenSolver solver(L.first); + + // find the index of the max eigenvalue + size_t maxIdx = 0; + for (auto i = 0; i < solver.eigenvalues().rows(); ++i) { + if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) + maxIdx = i; + } + // Store the max eigenvalue and its according eigenvector + const auto ev1 = solver.eigenvalues()(maxIdx).real(); + + Vector initial = Vector10::Random(); + PowerMethod pf(L.first, initial); + pf.compute(100, 1e-5); + EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); + auto actual2 = pf.eigenvector(); + const double ritzValue = actual2.dot(L.first * actual2); + const double ritzResidual = (L.first * actual2 - ritzValue * actual2).norm(); + EXPECT_DOUBLES_EQUAL(0, ritzResidual, 1e-5); +} + /* ************************************************************************* */ int main() { TestResult tr; From afb6ebb933308f755f6f4c9384925ed9c6bfc128 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Fri, 11 Dec 2020 01:01:27 -0500 Subject: [PATCH 19/22] Added the example graph in powerMethodExample.h --- gtsam/linear/tests/powerMethodExample.h | 67 +++++++++++++++++++ .../tests/testAcceleratedPowerMethod.cpp | 24 +------ gtsam/linear/tests/testPowerMethod.cpp | 24 +------ 3 files changed, 73 insertions(+), 42 deletions(-) create mode 100644 gtsam/linear/tests/powerMethodExample.h diff --git a/gtsam/linear/tests/powerMethodExample.h b/gtsam/linear/tests/powerMethodExample.h new file mode 100644 index 000000000..f80299386 --- /dev/null +++ b/gtsam/linear/tests/powerMethodExample.h @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * powerMethodExample.h + * + * @file powerMethodExample.h + * @date Nov 2020 + * @author Jing Wu + * @brief Create sparse and dense factor graph for + * PowerMethod/AcceleratedPowerMethod + */ + +#include + +#include + + +namespace gtsam { +namespace linear { +namespace test { +namespace example { + +/* ************************************************************************* */ +inline GaussianFactorGraph createSparseGraph() { + using symbol_shorthand::X; + // Let's make a scalar synchronization graph with 4 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + for (size_t j = 0; j < 3; j++) { + fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); + } + fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + + return fg; +} + +/* ************************************************************************* */ +inline GaussianFactorGraph createDenseGraph() { + using symbol_shorthand::X; + // Let's make a scalar synchronization graph with 10 nodes + GaussianFactorGraph fg; + auto model = noiseModel::Unit::Create(1); + // Iterate over nodes + for (size_t j = 0; j < 10; j++) { + // Each node has an edge with all the others + for (size_t i = 1; i < 10; i++) + fg.add(X(j), -I_1x1, X((j + i) % 10), I_1x1, Vector1::Zero(), model); + } + + return fg; +} + +/* ************************************************************************* */ + +} // namespace example +} // namespace test +} // namespace linear +} // namespace gtsam diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index c7c8e8a55..7c4a90936 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -33,7 +34,6 @@ using namespace std; using namespace gtsam; -using symbol_shorthand::X; /* ************************************************************************* */ TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { @@ -65,12 +65,7 @@ TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { /* ************************************************************************* */ TEST(AcceleratedPowerMethod, useFactorGraphSparse) { // Let's make a scalar synchronization graph with 4 nodes - GaussianFactorGraph fg; - auto model = noiseModel::Unit::Create(1); - for (size_t j = 0; j < 3; j++) { - fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); - } - fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph(); // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); @@ -105,20 +100,7 @@ TEST(AcceleratedPowerMethod, useFactorGraphSparse) { /* ************************************************************************* */ TEST(AcceleratedPowerMethod, useFactorGraphDense) { // Let's make a scalar synchronization graph with 10 nodes - GaussianFactorGraph fg; - auto model = noiseModel::Unit::Create(1); - // Each node has an edge with all the others - for (size_t j = 0; j < 10; j++) { - fg.add(X(j), -I_1x1, X((j + 1)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 2)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 3)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 4)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 5)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 6)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 7)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 8)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 9)%10 ), I_1x1, Vector1::Zero(), model); - } + GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph(); // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 7adfd0aa5..54d4c720d 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -33,7 +34,6 @@ using namespace std; using namespace gtsam; -using symbol_shorthand::X; /* ************************************************************************* */ TEST(PowerMethod, powerIteration) { @@ -63,12 +63,7 @@ TEST(PowerMethod, powerIteration) { /* ************************************************************************* */ TEST(PowerMethod, useFactorGraphSparse) { // Let's make a scalar synchronization graph with 4 nodes - GaussianFactorGraph fg; - auto model = noiseModel::Unit::Create(1); - for (size_t j = 0; j < 3; j++) { - fg.add(X(j), -I_1x1, X(j + 1), I_1x1, Vector1::Zero(), model); - } - fg.add(X(3), -I_1x1, X(0), I_1x1, Vector1::Zero(), model); // extra row + GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph(); // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); @@ -96,20 +91,7 @@ TEST(PowerMethod, useFactorGraphSparse) { /* ************************************************************************* */ TEST(PowerMethod, useFactorGraphDense) { // Let's make a scalar synchronization graph with 10 nodes - GaussianFactorGraph fg; - auto model = noiseModel::Unit::Create(1); - // Each node has an edge with all the others - for (size_t j = 0; j < 10; j++) { - fg.add(X(j), -I_1x1, X((j + 1)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 2)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 3)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 4)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 5)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 6)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 7)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 8)%10 ), I_1x1, Vector1::Zero(), model); - fg.add(X(j), -I_1x1, X((j + 9)%10 ), I_1x1, Vector1::Zero(), model); - } + GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph(); // Get eigenvalues and eigenvectors with Eigen auto L = fg.hessian(); From 55ad1f16a630f23a1f15ddeeb70f995b36ffa3ec Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Fri, 11 Dec 2020 01:01:40 -0500 Subject: [PATCH 20/22] Refined reference documentation --- gtsam/linear/AcceleratedPowerMethod.h | 10 ++++++---- gtsam/linear/PowerMethod.h | 12 +++++++----- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0699f237e..4cb25daf7 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -29,19 +29,21 @@ using Sparse = Eigen::SparseMatrix; * \brief Compute maximum Eigenpair with accelerated power method * * References : - * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns + * Hopkins University Press, 1996, pp.405-411 + * 2) Rosen, D. and Carlone, L., 2017, September. Computational * enhancements for certifiably correct SLAM. In Proceedings of the * International Conference on Intelligent Robots and Systems. - * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv - * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. * 58–67 * * It performs the following iteration: \f$ x_{k+1} = A * x_k - \beta * * x_{k-1} \f$ where A is the aim matrix we want to get eigenpair of, x is the * Ritz vector - * + * * Template argument Operator just needs multiplication operator * */ diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index a209c5779..8834777cc 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -35,19 +35,21 @@ using Sparse = Eigen::SparseMatrix; * \brief Compute maximum Eigenpair with power method * * References : - * 1) Rosen, D. and Carlone, L., 2017, September. Computational + * 1) G. Golub and C. V. Loan, Matrix Computations, 3rd ed. Baltimore, Johns + * Hopkins University Press, 1996, pp.405-411 + * 2) Rosen, D. and Carlone, L., 2017, September. Computational * enhancements for certifiably correct SLAM. In Proceedings of the * International Conference on Intelligent Robots and Systems. - * 2) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv - * 3) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated + * 4) C. de Sa, B. He, I. Mitliagkas, C. Ré, and P. Xu, “Accelerated * stochastic power iteration,” in Proc. Mach. Learn. Res., no. 84, 2018, pp. * 58–67 * - * It performs the following iteration: \f$ x_{k+1} = A * x_k \f$ + * It performs the following iteration: \f$ x_{k+1} = A * x_k \f$ * where A is the aim matrix we want to get eigenpair of, x is the * Ritz vector - * + * * Template argument Operator just needs multiplication operator * */ From 525dbb6058a0c4ba5cb7fcf7d9811dc6af43d110 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Fri, 11 Dec 2020 01:21:14 -0500 Subject: [PATCH 21/22] Make purturb static --- gtsam/sfm/ShonanAveraging.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index fab57c828..dfab2bf52 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -485,7 +485,7 @@ Sparse ShonanAveraging::computeA(const Matrix &S) const { // ref : Part III. C, Rosen, D. and Carlone, L., 2017, September. Computational // enhancements for certifiably correct SLAM. In Proceedings of the // International Conference on Intelligent Robots and Systems. -Vector perturb(const Vector &initialVector) { +static Vector perturb(const Vector &initialVector) { // generate a 0.03*||x_0||_2 as stated in David's paper int n = initialVector.rows(); Vector disturb = Vector::Random(n); From 9c781b605ff81533fdf9a4c145004a2ce27433a4 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 19 Jan 2021 00:07:21 -0500 Subject: [PATCH 22/22] Set estimateBeta() as optional --- gtsam/linear/AcceleratedPowerMethod.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 4cb25daf7..6653d43c9 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -69,10 +69,7 @@ class AcceleratedPowerMethod : public PowerMethod { previousVector_ = Vector::Zero(this->dim_); // initialize beta_ - if (!initialBeta) { - beta_ = estimateBeta(); - } else - beta_ = initialBeta; + beta_ = initialBeta; } /** @@ -97,8 +94,11 @@ class AcceleratedPowerMethod : public PowerMethod { return y; } - /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3) - double estimateBeta() const { + /** + * Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T + * is the iteration time to find beta with largest Rayleigh quotient + */ + double estimateBeta(const size_t T = 10) const { // set initial estimation of maxBeta Vector initVector = this->ritzVector_; const double up = initVector.dot( this->A_ * initVector ); @@ -109,7 +109,6 @@ class AcceleratedPowerMethod : public PowerMethod { std::vector betas; Matrix R = Matrix::Zero(this->dim_, 10); - const size_t T = 10; // run T times of iteration to find the beta that has the largest Rayleigh quotient for (size_t t = 0; t < T; t++) { // after each t iteration, reset the betas with the current maxBeta @@ -120,13 +119,14 @@ class AcceleratedPowerMethod : public PowerMethod { // initialize x0 and x00 in each iteration of each beta Vector x0 = initVector; Vector x00 = Vector::Zero(this->dim_); - // run 10 steps of accelerated power iteration with this beta + // run 10 steps of accelerated power iteration with this beta for (size_t j = 1; j < 10; j++) { if (j < 2) { R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]); R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]); } else { - R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2), betas[k]); + R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2), + betas[k]); } } // compute the Rayleigh quotient for the randomly sampled vector after