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 <Eigen/Core>
 #include <Eigen/Sparse>
-#include <Eigen/Array>
 #include <map>
 #include <string>
 #include <type_traits>
 #include <utility>
 #include <vector>
+#include <algorithm>
 
 namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
   /* ************************************************************************* */
 /// 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<const Vector> X(x, rows());
-    Eigen::Map<Vector> 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<Matrix> 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; i<ritz_vectors_.cols(); i++) {
-      if (converged(tol, ritz_vectors_.col(i))) converged += 1;
+      if (converged(tol, i)) {
+        num_converge += 1;
+      }
+      if (i>0 && i<ritz_vectors_.cols()-1) {
+        ritz_vectors_.col(i+1) = perform_op(i+1);
+      }
+    }
+    return num_converge;
+  }
+
+  size_t num_iterations() {
+    return m_niter_;
+  }
+
+  void sort_eigenpair() {
+    std::vector<std::pair<double, int>> pairs;
+    for(int i=0; i<ritz_conv_.size(); ++i) {
+      if (ritz_conv_(i)) pairs.push_back({ritz_val_(i), i});
+    }
+
+    std::sort(pairs.begin(), pairs.end(), [](const std::pair<double, int>& left, const std::pair<double, int>& 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<num_converged; ++j) {
+      sorted_ritz_val_(j) = pairs[j].first;
+      sorted_ritz_vectors_.col(j) = ritz_vectors_.col(pairs[j].second);
     }
-    return converged;
   }
 
   int compute(int maxit, double tol) {
     // Starting
-    int i, nconv = 0, nev_adj;
-    for (i = 0; i < maxit; i++) {
+    int i = 0;
+    int nconv = 0;
+    for (; i < maxit; i++) {
+      m_niter_ +=1;
       nconv = num_converged(tol);
       if (nconv >= 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 <gtsam/sfm/PowerMethod.h>
+#include <gtsam/sfm/ShonanAveraging.h>
+
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/slam/PowerMethod.h>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <iostream>
+#include <random>
 
 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);
 }
 
 /* ************************************************************************* */