Reformat code.
jingwuOUO
parent
758c4b061d
commit
b7f29a051a
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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();
|
||||
PowerMethod<Sparse> apf(A, S.row(0));
|
||||
apf.compute(20, 1e-4);
|
||||
EXPECT_LONGS_EQUAL(1, apf.eigenvectors().cols());
|
||||
EXPECT_LONGS_EQUAL(6, apf.eigenvectors().rows());
|
||||
|
||||
const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
|
||||
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));
|
||||
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();
|
||||
PowerMethod<Sparse> pf(A, initial);
|
||||
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);
|
||||
actual1(0) = abs(actual1(0));
|
||||
EXPECT(assert_equal(x1, actual1));
|
||||
const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
|
||||
Vector6 actual0 = pf.eigenvectors();
|
||||
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
|
||||
Matrix S = Matrix44::Zero();
|
||||
// PowerMethod<Matrix> pf(L.first, S.row(0));
|
||||
AcceleratedPowerMethod<Matrix> pf(L.first, S.row(0));
|
||||
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();
|
||||
PowerMethod<Matrix> pf(L.first, initial);
|
||||
pf.compute(20, 1e-4);
|
||||
cout << pf.eigenvalues() << endl;
|
||||
cout << pf.eigenvectors() << endl;
|
||||
EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues(), 1e-8);
|
||||
// auto actual2 = pf.eigenvectors();
|
||||
// EXPECT(assert_equal(ev2, actual2, 3e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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
|
||||
|
|
@ -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,
|
||||
Eigen::Index numLanczosVectors = 20) {
|
||||
double minEigenvalueNonnegativityTolerance = 10e-4) {
|
||||
|
||||
// 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;
|
||||
AcceleratedPowerMethod<Sparse> minShiftedOperator(C, S.row(0));
|
||||
const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A;
|
||||
AcceleratedPowerMethod<Sparse> minShiftedOperator(C, initial);
|
||||
|
||||
const int minConverged = minShiftedOperator.compute(
|
||||
maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue);
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
|
|
|||
Loading…
Reference in New Issue