From 8f3fbd2b9454cf7abde6b10e05caebfc291f996f Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 21 Sep 2020 15:16:29 -0400
Subject: [PATCH] Substantial refactor

---
 gtsam/sfm/PowerMethod.h             | 370 ++++++++++++----------------
 gtsam/sfm/ShonanAveraging.cpp       |   8 +-
 gtsam/sfm/tests/testPowerMethod.cpp |  56 +++--
 3 files changed, 202 insertions(+), 232 deletions(-)

diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h
index b8471e8e20..436195848d 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 <Eigen/Core>
 #include <Eigen/Sparse>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
 #include <map>
+#include <random>
 #include <string>
 #include <type_traits>
 #include <utility>
 #include <vector>
-#include <algorithm>
-#include <cmath>
-#include <chrono>
-#include <random>
 
 namespace gtsam {
 
@@ -41,9 +42,10 @@ using Sparse = Eigen::SparseMatrix<double>;
 /* ************************************************************************* */
 /// MINIMUM EIGENVALUE COMPUTATIONS
 
-struct PowerFunctor {
+// Template argument Operator just needs multiplication operator
+template <class Operator> struct PowerMethod {
   /**
-   * \brief Computer i-th Eigenpair with power method
+   * \brief Compute i-th Eigenpair with power method
    *
    * References :
    * 1) Rosen, D. and Carlone, L., 2017, September. Computational
@@ -52,143 +54,128 @@ 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
    *
    */
 
   // 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 nrRequired_; // number of eigenvalues required
 
-  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.
+  // flag for running power method or accelerated power method. If false, the
+  // former, vice versa.
   bool accelerated_;
 
   // a Polyak momentum term
   double beta_;
 
   // const int m_ncv_; // dimention of orthonormal basis subspace
-  size_t m_niter_; // number of iterations
+  size_t nrIterations_; // 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
+  Vector ritzValues_;       // all Ritz eigenvalues
+  Matrix ritzVectors_;      // all Ritz eigenvectors
+  Vector ritzConverged_;    // store whether the Ritz eigenpair converged
+  Vector sortedRitzValues_; // sorted converged eigenvalue
+  Matrix sortedRizVectors_; // sorted converged eigenvectors
 
 public:
- // Constructor
- explicit PowerFunctor(const Sparse& A, const Matrix& S, int nev, int ncv,
+  // Constructor
+  explicit PowerMethod(const Operator &A, const Matrix &S, int nrRequired = 1,
                        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;
-  }
+      : A_(A), dim_(A.rows()), nrRequired_(nrRequired),
+        accelerated_(accelerated), beta_(beta), nrIterations_(0) {
+    Vector x0;
+    Vector x00;
+    if (!S.isZero(0)) {
+      x0 = S.row(1);
+      x00 = S.row(0);
+    } else {
+      x0 = Vector::Random(dim_);
+      x00 = Vector::Random(dim_);
+    }
 
-  Vector perform_op(const Vector& x1, const Vector& x0, const double beta) const {
-    Vector x2 = A_ * x1 - beta * x0;
-    x2.normalize();
-    return x2;
-  }
+    // initialize Ritz eigen values
+    ritzValues_.resize(dim_);
+    ritzValues_.setZero();
 
-  Vector perform_op(const Vector& x1) const {
-    Vector x2 = A_ * x1;
-    x2.normalize();
-    return x2;
-  }
+    // initialize the Ritz converged vector
+    ritzConverged_.resize(dim_);
+    ritzConverged_.setZero();
 
-  Vector perform_op(int i) const {
+    // initialize Ritz eigen vectors
+    ritzVectors_.resize(dim_, nrRequired);
+    ritzVectors_.setZero();
     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;
-    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;
+      ritzVectors_.col(0) = update(x0, x00, beta_);
+      ritzVectors_.col(1) = update(ritzVectors_.col(0), x0, beta_);
+    } else {
+      ritzVectors_.col(0) = update(x0);
+      perturb(0);
     }
-    lo += hi >> 15;
-    if (lo > m_max) {
-      lo &= m_max;
-      ++lo;
+
+    // setting beta
+    if (accelerated_) {
+      Vector init_resid = ritzVectors_.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;
+      setBeta();
     }
-    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);
-    }
-    res.normalize();
-    return res;
+  Vector update(const Vector &x1, const Vector &x0, const double beta) const {
+    Vector y = A_ * x1 - beta * x0;
+    y.normalize();
+    return y;
+  }
+
+  Vector update(const Vector &x) const {
+    Vector y = A_ * x;
+    y.normalize();
+    return y;
+  }
+
+  Vector update(int i) const {
+    if (accelerated_) {
+      return update(ritzVectors_.col(i - 1), ritzVectors_.col(i - 2), beta_);
+    } else
+      return update(ritzVectors_.col(i - 1));
   }
 
   /// Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3)
   void setBeta() {
-    if (m_n_ < 10) return;
+    if (dim_ < 10)
+      return;
     double maxBeta = beta_;
     size_t maxIndex;
-    std::vector<double> betas = {2/3*maxBeta, 0.99*maxBeta, maxBeta, 1.01*maxBeta, 1.5*maxBeta};
+    std::vector<double> 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 ritzVectors;
+    ritzVectors.resize(dim_, 10);
+    ritzVectors.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]);
+          if (j < 2) {
+            Vector x0 = Vector::Random(dim_);
+            Vector x00 = Vector::Random(dim_);
+            ritzVectors.col(0) = update(x0, x00, betas[k]);
+            ritzVectors.col(1) = update(ritzVectors.col(0), x0, betas[k]);
+          } else {
+            ritzVectors.col(j) = update(ritzVectors.col(j - 1),
+                                        ritzVectors.col(j - 2), betas[k]);
           }
-          const Vector x = tmp_ritz_vectors.col(j);
+          const Vector x = ritzVectors.col(j);
           const double up = x.transpose() * A_ * x;
           const double down = x.transpose().dot(x);
           const double mu = up / down;
@@ -207,161 +194,122 @@ struct PowerFunctor {
   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::mt19937 generator(seed);
     std::uniform_real_distribution<double> uniform01(0.0, 1.0);
 
-    int n = m_n_;
+    int n = dim_;
     Vector disturb;
     disturb.resize(n);
     disturb.setZero();
-    for (int i =0; i<n; ++i) {
+    for (int i = 0; i < n; ++i) {
       disturb(i) = uniform01(generator);
     }
     disturb.normalize();
 
-    Vector x0 = ritz_vectors_.col(i);
+    Vector x0 = ritzVectors_.col(i);
     double magnitude = x0.norm();
-    ritz_vectors_.col(i) = x0 + 0.03 * magnitude * disturb;
-  }
-
-  void init(const Vector x0, const Vector x00) {
-    // initialzie ritz eigen values
-    ritz_val_.resize(m_n_);
-    ritz_val_.setZero();
-
-    // 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();
-    if (accelerated_){
-      ritz_vectors_.col(0) = perform_op(x0, x00);
-      ritz_vectors_.col(1) = perform_op(ritz_vectors_.col(0), x0);
-    } else {
-      ritz_vectors_.col(0) = perform_op(x0);
-      perturb(0);
-    }
-
-    // setting beta
-    if (accelerated_) {
-      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;
-      setBeta();
-    }
-    
-  }
-
-  void init() {
-    Vector x0;
-    Vector x00;
-    if (!S_.isZero(0)) {
-      x0 = S_.row(1);
-      x00 = S_.row(0);
-    } else {
-      x0 = random_vec(m_n_);
-      x00 = random_vec(m_n_);
-    }
-    init(x0, x00);
+    ritzVectors_.col(i) = x0 + 0.03 * magnitude * disturb;
   }
 
-  bool converged(double tol, int i) {
-    Vector x = ritz_vectors_.col(i);
+  // Perform power iteration on a single Ritz value
+  // Updates ritzValues_ and ritzConverged_
+  bool iterateOne(double tol, int i) {
+    const Vector x = ritzVectors_.col(i);
     double theta = x.transpose() * A_ * x;
 
-    // store the ritz eigen value
-    ritz_val_(i) = theta;
+    // store the Ritz eigen value
+    ritzValues_(i) = theta;
 
     // update beta
-    if (accelerated_) beta_ = std::max(beta_, theta * theta / 4);
+    if (accelerated_)
+      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;
+    const Vector diff = A_ * x - theta * x;
+    double error = diff.norm();
+    if (error < tol)
+      ritzConverged_(i) = 1;
     return error < tol;
   }
 
-  int num_converged(double tol) {
-    int num_converge = 0;
-    for (int i=0; i<ritz_vectors_.cols(); i++) {
-      if (converged(tol, i)) {
-        num_converge += 1;
+  // Perform power iteration on all Ritz values
+  // Updates ritzValues_, ritzVectors_, and ritzConverged_
+  int iterate(double tol) {
+    int nrConverged = 0;
+    for (int i = 0; i < nrRequired_; i++) {
+      if (iterateOne(tol, i)) {
+        nrConverged += 1;
       }
-      if (!accelerated_ && i<ritz_vectors_.cols()-1) {
-        ritz_vectors_.col(i+1) = perform_op(i+1);
-      } else if (accelerated_ && i>0 && i<ritz_vectors_.cols()-1) {
-        ritz_vectors_.col(i+1) = perform_op(i+1);
+      if (!accelerated_ && i < nrRequired_ - 1) {
+        ritzVectors_.col(i + 1) = update(i + 1);
+      } else if (accelerated_ && i > 0 && i < nrRequired_ - 1) {
+        ritzVectors_.col(i + 1) = update(i + 1);
       }
     }
-    return num_converge;
+    return nrConverged;
   }
 
-  size_t num_iterations() {
-    return m_niter_;
-  }
+  size_t nrIterations() { return nrIterations_; }
 
-  void sort_eigenpair() {
+  void sortEigenpairs() {
     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});
+    for (int i = 0; i < ritzConverged_.size(); ++i) {
+      if (ritzConverged_(i))
+        pairs.push_back({ritzValues_(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);
+    std::sort(pairs.begin(), pairs.end(),
+              [](const std::pair<double, int> &left,
+                 const std::pair<double, int> &right) {
+                return left.first < right.first;
+              });
+
+    // initialize sorted Ritz eigenvalues and eigenvectors
+    size_t nrConverged = pairs.size();
+    sortedRitzValues_.resize(nrConverged);
+    sortedRitzValues_.setZero();
+    sortedRizVectors_.resize(dim_, nrConverged);
+    sortedRizVectors_.setZero();
+
+    // fill sorted Ritz eigenvalues and eigenvectors with sorted index
+    for (size_t j = 0; j < nrConverged; ++j) {
+      sortedRitzValues_(j) = pairs[j].first;
+      sortedRizVectors_.col(j) = ritzVectors_.col(pairs[j].second);
     }
   }
 
   void reset() {
     if (accelerated_) {
-      ritz_vectors_.col(0) = perform_op(ritz_vectors_.col(m_n_-1-1), ritz_vectors_.col(m_n_-1-2));
-      ritz_vectors_.col(1) = perform_op(ritz_vectors_.col(0), ritz_vectors_.col(m_n_-1-1));
+      ritzVectors_.col(0) = update(ritzVectors_.col(dim_ - 1 - 1),
+                                   ritzVectors_.col(dim_ - 1 - 2), beta_);
+      ritzVectors_.col(1) =
+          update(ritzVectors_.col(0), ritzVectors_.col(dim_ - 1 - 1), beta_);
     } else {
-      ritz_vectors_.col(0) = perform_op(ritz_vectors_.col(m_n_-1));
+      ritzVectors_.col(0) = update(ritzVectors_.col(dim_ - 1));
     }
   }
 
   int compute(int maxit, double tol) {
     // Starting
     int i = 0;
-    int nconv = 0;
+    int nrConverged = 0;
     for (; i < maxit; i++) {
-      m_niter_ +=1;
-      nconv = num_converged(tol);
-      if (nconv >= m_nev_) break;
-      else reset();
+      nrIterations_ += 1;
+      nrConverged = iterate(tol);
+      if (nrConverged >= nrRequired_)
+        break;
+      else
+        reset();
     }
 
     // sort the result
-    sort_eigenpair();
+    sortEigenpairs();
 
-    return std::min(m_nev_, nconv);
+    return std::min(nrRequired_, nrConverged);
   }
 
-  Vector eigenvalues() {
-    return sorted_ritz_val_;
-  }
-
-  Matrix eigenvectors() {
-    return sorted_ritz_vectors_;
-  }
+  Vector eigenvalues() { return sortedRitzValues_; }
 
+  Matrix eigenvectors() { return sortedRizVectors_; }
 };
 
 } // namespace gtsam
diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp
index f287437d4a..e07636e104 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<Sparse> lmOperator(A, S, 1);
 
   const int lmConverged = lmOperator.compute(
       maxIterations, 1e-5);
@@ -503,8 +502,7 @@ static bool PowerMinimumEigenValue(
   }
 
   Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A;
-  PowerFunctor minShiftedOperator(C, S, 1, C.rows(), true);
-  minShiftedOperator.init();
+  PowerMethod<Sparse> minShiftedOperator(C, S, 1, true);
 
   const int minConverged = minShiftedOperator.compute(
       maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue);
@@ -516,7 +514,7 @@ static bool PowerMinimumEigenValue(
     *minEigenVector = minShiftedOperator.eigenvectors().col(0);
     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 4547c91b9d..2a96c44f93 100644
--- a/gtsam/sfm/tests/testPowerMethod.cpp
+++ b/gtsam/sfm/tests/testPowerMethod.cpp
@@ -18,36 +18,32 @@
  * @brief  Check eigenvalue and eigenvector computed by power method
  */
 
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/VectorSpace.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/sfm/PowerMethod.h>
-#include <gtsam/sfm/ShonanAveraging.h>
 
 #include <CppUnitLite/TestHarness.h>
 
 #include <Eigen/Core>
 #include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+
 #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");
+using symbol_shorthand::X;
 
 /* ************************************************************************* */
 TEST(PowerMethod, powerIteration) {
   // test power accelerated iteration
-  gtsam::Sparse A(6, 6);
+  Sparse A(6, 6);
   A.coeffRef(0, 0) = 6;
   Matrix S = Matrix66::Zero();
-  PowerFunctor apf(A, S, 1, A.rows(), true);
-  apf.init();
+  PowerMethod<Sparse> apf(A, S, 1, true);
   apf.compute(20, 1e-4);
   EXPECT_LONGS_EQUAL(6, apf.eigenvectors().cols());
   EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows());
@@ -61,10 +57,9 @@ TEST(PowerMethod, powerIteration) {
   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();
+  PowerMethod<Sparse> pf(A, S, 1);
   pf.compute(20, 1e-4);
-  // for power method, only 5 ritz vectors converge with 20 iteration
+  // for power method, only 5 ritz vectors converge with 20 iterations
   EXPECT_LONGS_EQUAL(5, pf.eigenvectors().cols());
   EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows());
 
@@ -75,6 +70,35 @@ TEST(PowerMethod, powerIteration) {
   EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues()(0), 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<Matrix> 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<Matrix> pf(L.first, S, 1);
+  pf.compute(20, 1e-4);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;