diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0e47c6a53..0ad87626b 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -34,7 +34,7 @@ using Sparse = Eigen::SparseMatrix; template class AcceleratedPowerMethod : public PowerMethod { /** - * \brief Compute maximum Eigenpair with power method + * \brief Compute maximum Eigenpair with accelerated power method * * References : * 1) Rosen, D. and Carlone, L., 2017, September. Computational @@ -46,8 +46,9 @@ class AcceleratedPowerMethod : public PowerMethod { * 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 + * 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 * */ @@ -59,46 +60,52 @@ class AcceleratedPowerMethod : public PowerMethod { // 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) + const Operator &A, const boost::optional initial = boost::none, + const double initialBeta = 0.0) : PowerMethod(A, initial) { Vector x0; // initialize ritz vector - x0 = initial ? Vector::Random(this->dim_) : initial.get(); + x0 = initial ? initial.get() : Vector::Random(this->dim_); Vector x00 = Vector::Random(this->dim_); x0.normalize(); x00.normalize(); // initialize Ritz eigen vector and previous vector - previousVector_ = update(x0, x00, beta_); - this->ritzVector_ = update(previousVector_, x0, beta_); - this->perturb(); + previousVector_ = powerIteration(x0, x00, beta_); + this->ritzVector_ = powerIteration(previousVector_, x0, beta_); - // 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(); + // initialize beta_ + if (!initialBeta) { + estimateBeta(); + } else { + beta_ = initialBeta; + } + } // Update the ritzVector with beta and previous two ritzVector - Vector update(const Vector &x1, const Vector &x0, const double beta) const { + Vector powerIteration(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_); + Vector powerIteration() const { + Vector y = powerIteration(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_; + void estimateBeta() { + // 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; + double maxBeta = mu * mu / 4; size_t maxIndex; std::vector betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta, 1.5 * maxBeta}; @@ -110,10 +117,10 @@ class AcceleratedPowerMethod : public PowerMethod { 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]); + R.col(0) = powerIteration(x0, x00, betas[k]); + R.col(1) = powerIteration(R.col(0), x0, betas[k]); } else { - R.col(j) = update(R.col(j - 1), R.col(j - 2), betas[k]); + R.col(j) = powerIteration(R.col(j - 1), R.col(j - 2), betas[k]); } } const Vector x = R.col(9); diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index 4807e0e6a..ee7a04429 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -46,8 +46,8 @@ class PowerMethod { size_t nrIterations_; // number of iterations - double ritzValue_; // all Ritz eigenvalues - Vector ritzVector_; // all Ritz eigenvectors + double ritzValue_; // Ritz eigenvalue + Vector ritzVector_; // Ritz eigenvector public: // Constructor @@ -55,85 +55,61 @@ class PowerMethod { const boost::optional initial = boost::none) : A_(A), dim_(A.rows()), nrIterations_(0) { Vector x0; - x0 = initial ? Vector::Random(dim_) : initial.get(); + x0 = initial ? initial.get() : Vector::Random(dim_); x0.normalize(); - // initialize Ritz eigen values + // initialize Ritz eigen value ritzValue_ = 0.0; // initialize Ritz eigen vectors ritzVector_ = Vector::Zero(dim_); - - ritzVector_.col(0) = update(x0); - perturb(); + ritzVector_ = powerIteration(x0); } // Update the vector by dot product with A_ - Vector update(const Vector &x) const { + Vector powerIteration(const Vector &x) const { Vector y = A_ * x; y.normalize(); return y; } // Update the vector by dot product with A_ - Vector update() const { return update(ritzVector_); } + Vector powerIteration() const { return powerIteration(ritzVector_); } - // 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); - // } - Vector disturb = Vector::Random(n); - 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) { + // After Perform power iteration on a single Ritz value, if the error is less + // than the tol then return true else false + bool converged(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(); + ritzValue_ = x.dot(A_ * x); + double error = (A_ * x - ritzValue_ * x).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) { + // Start the power/accelerated iteration, after updated the ritz vector, + // calculate the ritz error, repeat this operation until the ritz error converge + int compute(size_t maxIterations, double tol) { // Starting - int nrConverged = 0; + bool isConverged = false; - for (int i = 0; i < maxIterations; i++) { - nrIterations_ += 1; - ritzVector_ = update(); - nrConverged = iterateOne(tol); - if (nrConverged) break; + for (size_t i = 0; i < maxIterations; i++) { + ++nrIterations_ ; + ritzVector_ = powerIteration(); + isConverged = converged(tol); + if (isConverged) return isConverged; } - return std::min(1, nrConverged); + return isConverged; } // Return the eigenvalue - double eigenvalues() const { return ritzValue_; } + double eigenvalue() const { return ritzValue_; } // Return the eigenvector - const Vector eigenvectors() const { return ritzVector_; } + Vector eigenvector() const { return ritzVector_; } }; } // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 5f509c91e..60233fc6e 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -471,14 +471,40 @@ Sparse ShonanAveraging::computeA(const Values &values) const { return Lambda - Q_; } +/* ************************************************************************* */ +template +Sparse ShonanAveraging::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 +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, +// 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) +// (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, @@ -486,39 +512,39 @@ static bool PowerMinimumEigenValue( double minEigenvalueNonnegativityTolerance = 10e-4) { // a. Compute dominant eigenpair of S using power method - const boost::optional initial(S.row(0)); - PowerMethod lmOperator(A, initial); + PowerMethod lmOperator(A); - const int lmConverged = lmOperator.compute( + const bool lmConverged = lmOperator.compute( maxIterations, 1e-5); // Check convergence and bail out if necessary - if (lmConverged != 1) return false; + if (!lmConverged) return false; - const double lmEigenValue = lmOperator.eigenvalues(); + const double lmEigenValue = lmOperator.eigenvalue(); if (lmEigenValue < 0) { // The largest-magnitude eigenvalue is negative, and therefore also the // minimum eigenvalue, so just return this solution *minEigenValue = lmEigenValue; if (minEigenVector) { - *minEigenVector = lmOperator.eigenvectors(); + *minEigenVector = lmOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } const Sparse C = lmEigenValue * Matrix::Identity(A.rows(), A.cols()) - A; + const boost::optional initial = perturb(S.row(0)); AcceleratedPowerMethod minShiftedOperator(C, initial); - const int minConverged = minShiftedOperator.compute( + const bool minConverged = minShiftedOperator.compute( maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue); - if (minConverged != 1) return false; + if (!minConverged) return false; - *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalues(); + *minEigenValue = lmEigenValue - minShiftedOperator.eigenvalue(); if (minEigenVector) { - *minEigenVector = minShiftedOperator.eigenvectors(); + *minEigenVector = minShiftedOperator.eigenvector(); minEigenVector->normalize(); // Ensure that this is a unit vector } if (numIterations) *numIterations = minShiftedOperator.nrIterations(); @@ -669,13 +695,6 @@ static bool SparseMinimumEigenValue( return true; } -/* ************************************************************************* */ -template -Sparse ShonanAveraging::computeA(const Matrix &S) const { - auto Lambda = computeLambda(S); - return Lambda - Q_; -} - /* ************************************************************************* */ template double ShonanAveraging::computeMinEigenValue(const Values &values,