diff --git a/gtsam/linear/AcceleratedPowerMethod.h b/gtsam/linear/AcceleratedPowerMethod.h index 0ad87626b..2e54775d9 100644 --- a/gtsam/linear/AcceleratedPowerMethod.h +++ b/gtsam/linear/AcceleratedPowerMethod.h @@ -80,8 +80,7 @@ class AcceleratedPowerMethod : public PowerMethod { } else { beta_ = initialBeta; } - - } + } // Update the ritzVector with beta and previous two ritzVector Vector powerIteration(const Vector &x1, const Vector &x0, diff --git a/gtsam/linear/PowerMethod.h b/gtsam/linear/PowerMethod.h index ee7a04429..acb583296 100644 --- a/gtsam/linear/PowerMethod.h +++ b/gtsam/linear/PowerMethod.h @@ -89,14 +89,16 @@ class PowerMethod { // Return the number of iterations size_t nrIterations() const { return nrIterations_; } - // 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) { + // Start the power/accelerated iteration, after performing the + // power/accelerated power 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; i++) { - ++nrIterations_ ; + ++nrIterations_; ritzVector_ = powerIteration(); isConverged = converged(tol); if (isConverged) return isConverged; diff --git a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp index 474492475..b72556e0a 100644 --- a/gtsam/linear/tests/testAcceleratedPowerMethod.cpp +++ b/gtsam/linear/tests/testAcceleratedPowerMethod.cpp @@ -18,18 +18,16 @@ * @brief Check eigenvalue and eigenvector computed by accelerated power method */ +#include #include #include #include -#include #include - -#include +#include #include #include #include - #include #include @@ -70,7 +68,7 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { 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 + 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(); @@ -79,22 +77,26 @@ TEST(AcceleratedPowerMethod, useFactorGraph) { // 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); + 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; + 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::Random(); + Vector disturb = Vector4::Random(); + disturb.normalize(); + Vector initial = L.first.row(0); + double magnitude = initial.norm(); + initial += 0.03 * magnitude * disturb; AcceleratedPowerMethod apf(L.first, initial); apf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, apf.eigenvalue(), 1e-8); - EXPECT(assert_equal(-ev2, apf.eigenvector(), 3e-5)); + EXPECT(assert_equal(ev2, apf.eigenvector(), 3e-5)); } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testPowerMethod.cpp b/gtsam/linear/tests/testPowerMethod.cpp index 58d1ca0cc..7f6d1efa7 100644 --- a/gtsam/linear/tests/testPowerMethod.cpp +++ b/gtsam/linear/tests/testPowerMethod.cpp @@ -18,18 +18,16 @@ * @brief Check eigenvalue and eigenvector computed by power method */ +#include #include #include #include #include #include -#include - #include #include #include - #include #include @@ -68,7 +66,7 @@ TEST(PowerMethod, useFactorGraph) { 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 + 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(); @@ -77,12 +75,12 @@ TEST(PowerMethod, useFactorGraph) { // 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); + 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; + 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(); @@ -92,7 +90,7 @@ TEST(PowerMethod, useFactorGraph) { PowerMethod pf(L.first, initial); pf.compute(50, 1e-4); EXPECT_DOUBLES_EQUAL(ev1, pf.eigenvalue(), 1e-8); - auto actual2 = pf.eigenvector(); + auto actual2 = pf.eigenvector(); EXPECT(assert_equal(-ev2, actual2, 3e-5)); } diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 60233fc6e..50b38f75b 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -482,6 +482,9 @@ Sparse ShonanAveraging::computeA(const Matrix &S) const { // 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. Vector perturb(const Vector &initialVector) { // generate a 0.03*||x_0||_2 as stated in David's paper int n = initialVector.rows();