Merge pull request #533 from borglab/jing/powermethod
Added accelerated power method to compute eigenpair fastrelease/4.3a0
jingwuOUO
GitHub
commit
d267368b28
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
Loading…
Reference in New Issue