From b7f29a051a4b8f77c6bdd1e65490c579db0c1272 Mon Sep 17 00:00:00 2001 From: jingwuOUO Date: Tue, 13 Oct 2020 02:34:24 -0400 Subject: [PATCH] Reformat code. --- gtsam/linear/AcceleratedPowerMethod.h | 135 ++++++++++ gtsam/linear/PowerMethod.h | 140 ++++++++++ .../tests/testAcceleratedPowerMethod.cpp | 107 ++++++++ .../{sfm => linear}/tests/testPowerMethod.cpp | 55 ++-- gtsam/sfm/PowerMethod.h | 247 ------------------ gtsam/sfm/ShonanAveraging.cpp | 19 +- gtsam/sfm/ShonanAveraging.h | 3 +- 7 files changed, 422 insertions(+), 284 deletions(-) create mode 100644 gtsam/linear/AcceleratedPowerMethod.h create mode 100644 gtsam/linear/PowerMethod.h create mode 100644 gtsam/linear/tests/testAcceleratedPowerMethod.cpp rename gtsam/{sfm => linear}/tests/testPowerMethod.cpp (70%) delete mode 100644 gtsam/sfm/PowerMethod.h diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h new file mode 100644 index 000000000..2cbcaf67e --- /dev/null +++ b/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 +// #include +#include + +namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + +/* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS + +// Template argument Operator just needs multiplication operator +template +class AcceleratedPowerMethod : public 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 + * + */ + + 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 initial = boost::none) + : PowerMethod(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 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 diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h new file mode 100644 index 000000000..fe9aabb46 --- /dev/null +++ b/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 +#include + +#include +#include +#include +#include + +namespace gtsam { + +using Sparse = Eigen::SparseMatrix; + +/* ************************************************************************* */ +/// MINIMUM EIGENVALUE COMPUTATIONS + +// Template argument Operator just needs multiplication operator +template +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 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 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 diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp new file mode 100644 index 000000000..2a117d53b --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +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 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 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()(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 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); +} +/* ************************************************************************* */ diff --git a/gtsam/sfm/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp similarity index 70% rename from gtsam/sfm/tests/testPowerMethod.cpp rename to gtsam/linear/tests/testPowerMethod.cpp index d02906980..621286c85 100644 --- a/gtsam/sfm/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include @@ -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 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 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 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 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 pf(L.first, S.row(0)); - AcceleratedPowerMethod pf(L.first, S.row(0)); + size_t maxIdx = 0; + for (auto i =0; i= 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 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)); } /* ************************************************************************* */ diff --git a/gtsam/sfm/PowerMethod.h b/gtsam/sfm/PowerMethod.h deleted file mode 100644 index 5fa355aaa..000000000 --- a/gtsam/sfm/PowerMethod.h +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -using Sparse = Eigen::SparseMatrix; - -/* ************************************************************************* */ -/// MINIMUM EIGENVALUE COMPUTATIONS - -// Template argument Operator just needs multiplication operator -template -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 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 AcceleratedPowerMethod : public PowerMethod { - double beta_ = 0; // a Polyak momentum term - - Vector previousVector_; // store previous vector - - public: - // Constructor - explicit AcceleratedPowerMethod(const Operator &A, const Vector &initial) - : PowerMethod(A, initial) { - Vector x0 = initial; - // initialize ritz vector - x0 = x0.isZero(0) ? Vector::Random(PowerMethod::dim_) : x0; - Vector x00 = Vector::Random(PowerMethod::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::dim_ < 10) return; - double maxBeta = beta_; - size_t maxIndex; - std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, - 1.01 * maxBeta, 1.5 * maxBeta}; - - Matrix tmpRitzVectors; - tmpRitzVectors.resize(PowerMethod::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::dim_); - Vector x00 = Vector::Random(PowerMethod::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 diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 9ebee4f9f..5f509c91e 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -471,16 +471,23 @@ Sparse ShonanAveraging::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 lmOperator(A, S.row(0)); + const boost::optional initial(S.row(0)); + PowerMethod 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 minShiftedOperator(C, S.row(0)); + const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + AcceleratedPowerMethod minShiftedOperator(C, initial); const int minConverged = minShiftedOperator.compute( maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index 9d117a266..49659386f 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -26,7 +26,8 @@ #include #include #include -#include +#include +#include #include #include