12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Completed and tested power method with unittest

release/4.3a0
jingwuOUO 2020-09-14 01:58:44 -04:00
parent 2afbaaaf44
commit e02633c745
2 changed files with 149 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
gtsam/sfm/PowerMethod.h
View File

@ -24,14 +24,17 @@
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/Array>
#include <map>
#include <string>
#include <type_traits>
#include <utility>
#include <vector>
#include <algorithm>
namespace gtsam {
using Sparse = Eigen::SparseMatrix<double>;
/* ************************************************************************* */
/// MINIMUM EIGENVALUE COMPUTATIONS
@ -39,46 +42,63 @@ namespace gtsam {
* 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 {
struct PowerFunctor {
// 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_; // dimension of Matrix A
const int m_nev_; // number of eigenvalues required
const int m_n_;
const int m_nev_;
const int m_ncv_;
Vector ritz_val_;
Matrix ritz_vectors_;
// a Polyak momentum term
double beta_;
// const int m_ncv_; // dimention of orthonormal basis subspace
size_t m_niter_; // number of iterations
private:
Vector ritz_val_; // all ritz eigenvalues
Matrix ritz_vectors_; // all ritz eigenvectors
Vector ritz_conv_; // store whether the ritz eigenpair converged
Vector sorted_ritz_val_; // sorted converged eigenvalue
Matrix sorted_ritz_vectors_; // sorted converged eigenvectors
public:
// Constructor
explicit MatrixProdFunctor(const Sparse &A, int nev, int ncv,
double sigma = 0)
explicit PowerFunctor(const Sparse &A, int nev, int ncv,
double beta = 0)
: A_(A),
m_n_(A.rows()),
m_nev_(nev),
m_ncv_(ncv > m_n_ ? m_n_ : ncv),
sigma_(sigma) {}
// m_ncv_(ncv > m_n_ ? m_n_ : ncv),
beta_(beta),
m_niter_(0)
{}
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());
Vector perform_op(const Vector& x1, const Vector& x0) const {
// Do the multiplication using wrapped Eigen vectors
Y = A_ * X + sigma_ * X;
Vector x2 = A_ * x1 - beta_ * x0;
x2.normalize();
return x2;
}
Vector perform_op(int i) const {
// Do the multiplication using wrapped Eigen vectors
Vector x1 = ritz_vectors_.col(i-1);
Vector x2 = ritz_vectors_.col(i-2);
return perform_op(x1, x2);
}
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;
// long m_rand = m_n_ ? (m_n_ & m_max) : 1;
unsigned long lo, hi;
lo = m_a * (long)(seed & 0xFFFF);
@ -103,62 +123,119 @@ struct MatrixProdFunctor {
long m_rand = next_long_rand(m_rand);
res[i] = double(m_rand) / double(m_max) - double(0.5);
}
res.normalize();
return res;
}
void init(Vector data) {
ritz_val_.resize(m_ncv_);
void init(const Vector x0, const Vector x00) {
// initialzie ritz eigen values
ritz_val_.resize(m_n_);
ritz_val_.setZero();
ritz_vectors_.resize(m_ncv_, m_nev_);
// initialzie the ritz converged vector
ritz_conv_.resize(m_n_);
ritz_conv_.setZero();
// initialzie ritz eigen vectors
ritz_vectors_.resize(m_n_, m_n_);
ritz_vectors_.setZero();
Eigen::Map<Matrix> v0(init_resid.data(), m_n_);
ritz_vectors_.col(0) = perform_op(x0, x00);
ritz_vectors_.col(1) = perform_op(ritz_vectors_.col(0), x0);
// setting beta
Vector init_resid = ritz_vectors_.col(0);
const double up = init_resid.transpose() * A_ * init_resid;
const double down = init_resid.transpose().dot(init_resid);
const double mu = up/down;
beta_ = mu*mu/4;
}
void init() {
Vector init_resid = random_vec(m_n_);
init(init_resid);
Vector x0 = random_vec(m_n_);
Vector x00 = random_vec(m_n_);
init(x0, x00);
}
bool converged(double tol, const Vector x) {
bool converged(double tol, int i) {
Vector x = ritz_vectors_.col(i);
double theta = x.transpose() * A_ * x;
// store the ritz eigen value
ritz_val_(i) = theta;
// update beta
beta_ = std::max(beta_, theta * theta / 4);
Vector diff = A_ * x - theta * x;
double error = diff.lpNorm<1>();
if (error < tol) ritz_conv_(i) = 1;
return error < tol;
}
int num_converged(double tol) {
int converge = 0;
int num_converge = 0;
for (int i=0; i<ritz_vectors_.cols(); i++) {
if (converged(tol, ritz_vectors_.col(i))) converged += 1;
if (converged(tol, i)) {
num_converge += 1;
}
if (i>0 && i<ritz_vectors_.cols()-1) {
ritz_vectors_.col(i+1) = perform_op(i+1);
}
}
return num_converge;
}
size_t num_iterations() {
return m_niter_;
}
void sort_eigenpair() {
std::vector<std::pair<double, int>> pairs;
for(int i=0; i<ritz_conv_.size(); ++i) {
if (ritz_conv_(i)) pairs.push_back({ritz_val_(i), i});
}
std::sort(pairs.begin(), pairs.end(), [](const std::pair<double, int>& left, const std::pair<double, int>& right) {
return left.first < right.first;
});
// initialzie sorted ritz eigenvalues and eigenvectors
size_t num_converged = pairs.size();
sorted_ritz_val_.resize(num_converged);
sorted_ritz_val_.setZero();
sorted_ritz_vectors_.resize(m_n_, num_converged);
sorted_ritz_vectors_.setZero();
// fill sorted ritz eigenvalues and eigenvectors with sorted index
for(size_t j=0; j<num_converged; ++j) {
sorted_ritz_val_(j) = pairs[j].first;
sorted_ritz_vectors_.col(j) = ritz_vectors_.col(pairs[j].second);
}
return converged;
}
int compute(int maxit, double tol) {
// Starting
int i, nconv = 0, nev_adj;
for (i = 0; i < maxit; i++) {
int i = 0;
int nconv = 0;
for (; i < maxit; i++) {
m_niter_ +=1;
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;
// sort the result
sort_eigenpair();
return std::min(m_nev_, nconv);
}
Vector eigenvalues() {
return ritz_val_;
return sorted_ritz_val_;
}
Matrix eigenvectors() {
return ritz_vectors_;
return sorted_ritz_vectors_;
}
};

35
gtsam/sfm/tests/testPowerMethod.cpp
View File

@ -18,16 +18,45 @@
* @brief Check eigenvalue and eigenvector computed by power method
*/
#include <gtsam/sfm/PowerMethod.h>
#include <gtsam/sfm/ShonanAveraging.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/PowerMethod.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <random>
using namespace std;
using namespace gtsam;
ShonanAveraging3 fromExampleName(
const std::string &name,
ShonanAveraging3::Parameters parameters = ShonanAveraging3::Parameters()) {
string g2oFile = findExampleDataFile(name);
return ShonanAveraging3(g2oFile, parameters);
}
static const ShonanAveraging3 kShonan = fromExampleName("toyExample.g2o");
/* ************************************************************************* */
TEST(PowerMethod, initialize) {
gtsam::Sparse A;
MatrixProdFunctor(A, 1, A.rows());
gtsam::Sparse A(6, 6);
A.coeffRef(0, 0) = 6;
PowerFunctor pf(A, 1, A.rows());
pf.init();
pf.compute(20, 1e-4);
EXPECT_LONGS_EQUAL(6, pf.eigenvectors().cols());
EXPECT_LONGS_EQUAL(6, pf.eigenvectors().rows());
const Vector6 x1 = (Vector(6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished();
Vector6 actual = pf.eigenvectors().col(0);
actual(0) = abs(actual(0));
EXPECT(assert_equal(x1, actual));
const double ev1 = 6.0;
EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalues()(0), 1e-5);
}
/* ************************************************************************* */