Add acc power method in Alg6

gtsam/sfm/PowerMethod.h

@@ -0,0 +1,166 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 <Eigen/Array>
+#include <map>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+  /* ************************************************************************* */
+/// MINIMUM EIGENVALUE COMPUTATIONS
+
+/** 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
+ * nontrivial function, perform_op(x,y), that computes and returns the product
+ * y = (A + sigma*I) x */
+struct MatrixProdFunctor {
+  // 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_;
+  const int m_nev_;
+  const int m_ncv_;
+  Vector ritz_val_;
+  Matrix ritz_vectors_;
+
+  // Constructor
+  explicit MatrixProdFunctor(const Sparse &A, int nev, int ncv,
+                             double sigma = 0)
+      : A_(A),
+        m_n_(A.rows()),
+        m_nev_(nev),
+        m_ncv_(ncv > m_n_ ? m_n_ : ncv),
+        sigma_(sigma) {}
+
+  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());
+
+    // Do the multiplication using wrapped Eigen vectors
+    Y = A_ * X + sigma_ * X;
+  }
+
+  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;
+    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;
+    }
+    lo += hi >> 15;
+    if (lo > m_max) {
+      lo &= m_max;
+      ++lo;
+    }
+    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);
+    }
+    return res;
+  }
+
+  void init(Vector data) {
+    ritz_val_.resize(m_ncv_);
+    ritz_val_.setZero();
+    ritz_vectors_.resize(m_ncv_, m_nev_);
+    ritz_vectors_.setZero();
+    Eigen::Map<Matrix> v0(init_resid.data(), m_n_);
+  }
+
+  void init() {
+    Vector init_resid = random_vec(m_n_);
+    init(init_resid);
+  }
+
+  bool converged(double tol, const Vector x) {
+    double theta = x.transpose() * A_ * x;
+    Vector diff = A_ * x - theta * x;
+    double error = diff.lpNorm<1>();
+    return error < tol;
+  }
+
+  int num_converged(double tol) {
+    int converge = 0;
+    for (int i=0; i<ritz_vectors_.cols(); i++) {
+      if (converged(tol, ritz_vectors_.col(i))) converged += 1;
+    }
+    return converged;
+  }
+
+  int compute(int maxit, double tol) {
+    // Starting
+    int i, nconv = 0, nev_adj;
+    for (i = 0; i < maxit; i++) {
+      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;
+
+    return std::min(m_nev_, nconv);
+  }
+
+  Vector eigenvalues() {
+    return ritz_val_;
+  }
+
+  Matrix eigenvectors() {
+    return ritz_vectors_;
+  }
+
+};
+
+} // namespace gtsam

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>
@@ -470,38 +471,55 @@ Sparse ShonanAveraging<d>::computeA(const Values &values) const {
   return Lambda - Q_;
 }
 
-/* ************************************************************************* */
+// Alg.6 from paper Distributed Certifiably Correct Pose-Graph Optimization
-/// MINIMUM EIGENVALUE COMPUTATIONS
+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,
+    Eigen::Index numLanczosVectors = 20) {
 
-/** This is a lightweight struct used in conjunction with Spectra to compute
+  // a. Compute dominant eigenpair of S using power method
- * the minimum eigenvalue and eigenvector of a sparse matrix A; it has a single
+  MatrixProdFunctor lmOperator(A, 1, std::min(numLanczosVectors, A.rows()));
- * nontrivial function, perform_op(x,y), that computes and returns the product
+  lmOperator.init();
- * y = (A + sigma*I) x */
-struct MatrixProdFunctor {
-  // Const reference to an externally-held matrix whose minimum-eigenvalue we
-  // want to compute
-  const Sparse &A_;
 
-  // Spectral shift
+  const int lmConverged = lmEigenValueSolver.compute(
-  double sigma_;
+      maxIterations, 1e-4);
 
-  // Constructor
+  // Check convergence and bail out if necessary
-  explicit MatrixProdFunctor(const Sparse &A, double sigma = 0)
+  if (lmConverged != 1) return false;
-      : A_(A), sigma_(sigma) {}
 
-  int rows() const { return A_.rows(); }
+  const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
-  int cols() const { return A_.cols(); }
 
-  // Matrix-vector multiplication operation
+  if (lmEigenValue < 0) {
-  void perform_op(const double *x, double *y) const {
+    // The largest-magnitude eigenvalue is negative, and therefore also the
-    // Wrap the raw arrays as Eigen Vector types
+    // minimum eigenvalue, so just return this solution
-    Eigen::Map<const Vector> X(x, rows());
+    *minEigenValue = lmEigenValue;
-    Eigen::Map<Vector> Y(y, rows());
+    if (minEigenVector) {
-
+      *minEigenVector = lmEigenValueSolver.eigenvectors().col(0);
-    // Do the multiplication using wrapped Eigen vectors
+      minEigenVector->normalize();  // Ensure that this is a unit vector
-    Y = A_ * X + sigma_ * X;
     }
-};
+    return true;
+  }
+
+  Matrix C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A;
+  MatrixProdFunctor minShiftedOperator(
+      C, 1, std::min(numLanczosVectors, A.rows()));
+  minShiftedOperator.init();
+
+  const int minConverged = minShiftedOperator.compute(
+      maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue);
+
+  if (minConverged != 1) return false;
+
+  *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues()(0);
+  if (minEigenVector) {
+    *minEigenVector = minShiftedOperator.eigenvectors().col(0);
+    minEigenVector->normalize();  // Ensure that this is a unit vector
+  }
+  if (numIterations) *numIterations = minShiftedOperator.num_iterations();
+  return true;
+}
 
 /// Function to compute the minimum eigenvalue of A using Lanczos in Spectra.
 /// This does 2 things:

gtsam/sfm/tests/testPowerMethod.cpp

@@ -0,0 +1,38 @@
+/* ----------------------------------------------------------------------------
+
+ * 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/slam/PowerMethod.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(PowerMethod, initialize) {
+  gtsam::Sparse A;
+  MatrixProdFunctor(A, 1, A.rows());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */