Reformat code.

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 <gtsam/base/Matrix.h>
+// #include <gtsam/base/Vector.h>
+#include <gtsam/linear/PowerMethod.h>
+
+namespace gtsam {
+
+using Sparse = Eigen::SparseMatrix<double>;
+
+/* ************************************************************************* */
+/// MINIMUM EIGENVALUE COMPUTATIONS
+
+// Template argument Operator just needs multiplication operator
+template <class Operator>
+class AcceleratedPowerMethod : public PowerMethod<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 + \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<Vector> initial = boost::none)
+      : PowerMethod<Operator>(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<double> 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

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 <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>;
+
+/* ************************************************************************* */
+/// MINIMUM EIGENVALUE COMPUTATIONS
+
+// 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_;   // all Ritz eigenvalues
+  Vector ritzVector_;  // all Ritz eigenvectors
+
+ public:
+  // Constructor
+  explicit PowerMethod(const Operator &A,
+                       const boost::optional<Vector> 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<double> 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

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

gtsam/linear/tests/testPowerMethod.cpp

@@ -22,7 +22,7 @@
 #include <gtsam/base/VectorSpace.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/sfm/PowerMethod.h>
+#include <gtsam/linear/PowerMethod.h>
 
 #include <CppUnitLite/TestHarness.h>
 
@@ -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();
+  A.coeffRef(0, 0) = 5;
-  PowerMethod<Sparse> apf(A, S.row(0));
+  A.coeffRef(0, 0) = 4;
-  apf.compute(20, 1e-4);
+  A.coeffRef(0, 0) = 3;
-  EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols());
+  A.coeffRef(0, 0) = 2;
-  EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows());
+  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();
+  PowerMethod<Sparse> pf(A, initial);
-  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<Sparse> pf(A, S.row(0));
   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);
+  const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
-  actual1(0) = abs(actual1(0));
+  Vector6 actual0 = pf.eigenvectors();
-  EXPECT(assert_equal(x1, actual1));
+  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<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);
@@ -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
+  size_t maxIdx = 0;
-  Matrix S = Matrix44::Zero();
+  for (auto i =0; i<solver.eigenvalues().rows(); ++i) {
-  // PowerMethod<Matrix> pf(L.first, S.row(0));
+    if (solver.eigenvalues()(i).real() >= solver.eigenvalues()(maxIdx).real()) maxIdx = i;
-  AcceleratedPowerMethod<Matrix> pf(L.first, S.row(0));
+  }
+  // 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<Matrix> pf(L.first, initial);
   pf.compute(20, 1e-4);
-  cout << pf.eigenvalues() << endl;
+  EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-8);
-  cout << pf.eigenvectors() << endl;
+  // auto actual2  = pf.eigenvectors();
+  // EXPECT(assert_equal(ev2, actual2, 3e-5));
 }
 
 /* ************************************************************************* */

gtsam/sfm/PowerMethod.h

@@ -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 <gtsam/base/Matrix.h>
-#include <gtsam/base/Vector.h>
-#include <gtsam/dllexport.h>
-
-#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>
-
-namespace gtsam {
-
-using Sparse = Eigen::SparseMatrix<double>;
-
-/* ************************************************************************* */
-/// MINIMUM EIGENVALUE COMPUTATIONS
-
-// Template argument Operator just needs multiplication operator
-template <class Operator>
-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<double> 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 Operator>
-class AcceleratedPowerMethod : public PowerMethod<Operator> {
-  double beta_ = 0;  // a Polyak momentum term
-
-  Vector previousVector_;  // store previous vector
-
- public:
-  // Constructor
-  explicit AcceleratedPowerMethod(const Operator &A, const Vector &initial)
-      : PowerMethod<Operator>(A, initial) {
-    Vector x0 = initial;
-    // initialize ritz vector
-    x0 = x0.isZero(0) ? Vector::Random(PowerMethod<Operator>::dim_) : x0;
-    Vector x00 = Vector::Random(PowerMethod<Operator>::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<Operator>::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};
-
-    Matrix tmpRitzVectors;
-    tmpRitzVectors.resize(PowerMethod<Operator>::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<Operator>::dim_);
-            Vector x00 = Vector::Random(PowerMethod<Operator>::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

gtsam/sfm/ShonanAveraging.cpp

@@ -471,16 +471,23 @@ Sparse ShonanAveraging<d>::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,
+    double minEigenvalueNonnegativityTolerance = 10e-4) {
-    Eigen::Index numLanczosVectors = 20) {
 
   // a. Compute dominant eigenpair of S using power method
-  PowerMethod<Sparse> lmOperator(A, S.row(0));
+  const boost::optional<Vector> initial(S.row(0));
+  PowerMethod<Sparse> 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;
+  const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A;
-  AcceleratedPowerMethod<Sparse> minShiftedOperator(C, S.row(0));
+  AcceleratedPowerMethod<Sparse> minShiftedOperator(C, initial);
 
   const int minConverged = minShiftedOperator.compute(
       maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue);

gtsam/sfm/ShonanAveraging.h

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