diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h new file mode 100644 index 000000000..6653d43c9 --- /dev/null +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -0,0 +1,176 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + +/** + * \brief Compute maximum Eigenpair with accelerated power method + * + * References : + * 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. + * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 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 + * + */ +template +class AcceleratedPowerMethod : public PowerMethod { + + 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, + double initialBeta = 0.0) + : PowerMethod(A, initial) { + // initialize Ritz eigen vector and previous vector + this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); + this->ritzVector_.normalize(); + previousVector_ = Vector::Zero(this->dim_); + + // initialize beta_ + beta_ = initialBeta; + } + + /** + * 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; + y.normalize(); + return y; + } + + /** + * 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), 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 ); + const double down = initVector.dot(initVector); + const double mu = up / down; + double maxBeta = mu * mu / 4; + size_t maxIndex; + std::vector betas; + + Matrix R = Matrix::Zero(this->dim_, 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) = 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]); + } + } + // 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 + 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. + */ + bool compute(size_t maxIterations, double tol) { + // Starting + bool isConverged = false; + + for (size_t i = 0; i < maxIterations && !isConverged; 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); + } + + return isConverged; + } +}; + +} // namespace gtsam diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h new file mode 100644 index 000000000..8834777cc --- /dev/null +++ b/gtsam/linear/PowerMethod.h @@ -0,0 +1,152 @@ +/* ---------------------------------------------------------------------------- + + * 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; + +/** + * \brief Compute maximum Eigenpair with power method + * + * References : + * 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. + * 3) Yulun Tian and Kasra Khosoussi and David M. Rosen and Jonathan P. How, + * 2020, Aug, Distributed Certifiably Correct Pose-Graph Optimization, Arxiv + * 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$ + * 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 Operator &A_; + + const int dim_; // dimension of Matrix A + + size_t nrIterations_; // number of iterations + + double ritzValue_; // Ritz eigenvalue + Vector ritzVector_; // Ritz eigenvector + + public: + /// @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) { + Vector x0; + x0 = initial ? initial.get() : Vector::Random(dim_); + x0.normalize(); + + // initialize Ritz eigen value + ritzValue_ = 0.0; + + // initialize Ritz eigen vector + ritzVector_ = powerIteration(x0); + } + + /** + * 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 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 + */ + bool converged(double tol) const { + const Vector x = ritzVector_; + // store the Ritz eigen value + const double ritzValue = x.dot(A_ * x); + const double error = (A_ * x - ritzValue * x).norm(); + return error < tol; + } + + /// 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. + */ + bool compute(size_t maxIterations, double tol) { + // Starting + bool isConverged = false; + + 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); + } + + return isConverged; + } + + /// Return the eigenvalue + double eigenvalue() const { return ritzValue_; } + + /// Return the eigenvector + Vector eigenvector() const { return ritzVector_; } +}; + +} // namespace gtsam 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 new file mode 100644 index 000000000..7c4a90936 --- /dev/null +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -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 + + * -------------------------------------------------------------------------- */ + +/** + * 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 +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, acceleratedPowerIteration) { + // test power iteration, beta is set to 0 + Sparse A(6, 6); + A.coeffRef(0, 0) = 6; + 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 = (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(100, 1e-5); + EXPECT_LONGS_EQUAL(6, apf.eigenvector().rows()); + + Vector6 actual1 = apf.eigenvector(); + 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); +} + +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, useFactorGraphSparse) { + // Let's make a scalar synchronization graph with 4 nodes + GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph(); + + // 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 = 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(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); +} + +/* ************************************************************************* */ +TEST(AcceleratedPowerMethod, useFactorGraphDense) { + // Let's make a scalar synchronization graph with 10 nodes + GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph(); + + // 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; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp new file mode 100644 index 000000000..54d4c720d --- /dev/null +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST(PowerMethod, powerIteration) { + // test power iteration, beta is set to 0 + Sparse A(6, 6); + A.coeffRef(0, 0) = 6; + 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 = (Vector(6) << 0.24434602, 0.22829942, 0.70094486, 0.15463092, 0.55871359, + 0.2465342).finished(); + PowerMethod pf(A, initial); + pf.compute(100, 1e-5); + EXPECT_LONGS_EQUAL(6, pf.eigenvector().rows()); + + 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); +} + +/* ************************************************************************* */ +TEST(PowerMethod, useFactorGraphSparse) { + // Let's make a scalar synchronization graph with 4 nodes + GaussianFactorGraph fg = gtsam::linear::test::example::createSparseGraph(); + + // 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 = Vector4::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); +} + +/* ************************************************************************* */ +TEST(PowerMethod, useFactorGraphDense) { + // Let's make a scalar synchronization graph with 10 nodes + GaussianFactorGraph fg = gtsam::linear::test::example::createDenseGraph(); + + // 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; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index a0e615e9b..53f8f6b6c 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -487,8 +488,88 @@ 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 +// 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. +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); + 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, +// there are two parts +// in this algorithm: +// (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, + size_t maxIterations = 1000, + double minEigenvalueNonnegativityTolerance = 10e-4) { + + // a. Compute dominant eigenpair of S using power method + PowerMethod pmOperator(A); + + const bool pmConverged = pmOperator.compute( + maxIterations, 1e-5); + + // Check convergence and bail out if necessary + if (!pmConverged) return false; + + const double pmEigenValue = pmOperator.eigenvalue(); + + if (pmEigenValue < 0) { + // The largest-magnitude eigenvalue is negative, and therefore also the + // minimum eigenvalue, so just return this solution + minEigenValue = pmEigenValue; + if (minEigenVector) { + *minEigenVector = pmOperator.eigenvector(); + minEigenVector->normalize(); // Ensure that this is a unit vector + } + return true; + } + + const Sparse C = pmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + const boost::optional initial = perturb(S.row(0)); + AcceleratedPowerMethod apmShiftedOperator(C, initial); + + const bool minConverged = apmShiftedOperator.compute( + maxIterations, minEigenvalueNonnegativityTolerance / pmEigenValue); + + if (!minConverged) return false; + + minEigenValue = pmEigenValue - apmShiftedOperator.eigenvalue(); + if (minEigenVector) { + *minEigenVector = apmShiftedOperator.eigenvector(); + minEigenVector->normalize(); // Ensure that this is a unit vector + } + if (numIterations) *numIterations = apmShiftedOperator.nrIterations(); + 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 @@ -634,13 +715,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, @@ -658,6 +732,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 = 0; + 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 e8a828be8..0a8bdc8c1 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -26,6 +26,8 @@ #include #include #include +#include +#include #include #include @@ -250,6 +252,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 06e909c61..556289d22 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -188,9 +188,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)