Merge pull request #533 from borglab/jing/powermethod

Added accelerated power method to compute eigenpair fast

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 <gtsam/linear/PowerMethod.h>
+
+namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/**
+ * \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 Operator>
+class AcceleratedPowerMethod : public PowerMethod<Operator> {
+
+  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<Vector> initial = boost::none,
+      double initialBeta = 0.0)
+      : PowerMethod<Operator>(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<double> 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

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 <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+
+#include <Eigen/Core>
+#include <Eigen/Sparse>
+#include <random>
+#include <vector>
+
+namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/**
+ * \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 Operator>
+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<Vector> 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

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 <gtsam/inference/Symbol.h>
+
+#include <iostream>
+
+
+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

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 <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/AcceleratedPowerMethod.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/tests/powerMethodExample.h>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+#include <iostream>
+#include <random>
+
+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<Sparse> 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<Matrix> 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<Matrix> 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<Matrix> 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<Matrix> 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);
+}
+/* ************************************************************************* */

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 <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/PowerMethod.h>
+#include <gtsam/linear/tests/powerMethodExample.h>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+#include <iostream>
+#include <random>
+
+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<Sparse> 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<Matrix> 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<Matrix> 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<Matrix> 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<Matrix> 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);
+}
+/* ************************************************************************* */

gtsam/sfm/ShonanAveraging.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <SymEigsSolver.h>
+#include <cmath>
 #include <gtsam/linear/PCGSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -487,8 +488,88 @@ Sparse ShonanAveraging<d>::computeA(const Values &values) const {
   return Lambda - Q_;
 }
 
+/* ************************************************************************* */
+template <size_t d>
+Sparse ShonanAveraging<d>::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<Sparse> 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<Vector> initial = perturb(S.row(0));
+  AcceleratedPowerMethod<Sparse> 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 <size_t d>
-Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
-  auto Lambda = computeLambda(S);
-  return Lambda - Q_;
-}
-
 /* ************************************************************************* */
 template <size_t d>
 double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
@@ -658,6 +732,23 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
   return minEigenValue;
 }
 
+/* ************************************************************************* */
+template <size_t d>
+double ShonanAveraging<d>::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 <size_t d>
 std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(

gtsam/sfm/ShonanAveraging.h

@@ -26,6 +26,8 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
 #include <gtsam/sfm/BinaryMeasurement.h>
+#include <gtsam/linear/PowerMethod.h>
+#include <gtsam/linear/AcceleratedPowerMethod.h>
 #include <gtsam/slam/dataset.h>
 
 #include <Eigen/Sparse>
@@ -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;
 

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)