From c99a8e1c1deabb6cf03bd57f50c1dbe367109b30 Mon Sep 17 00:00:00 2001 From: Martin Valgur Date: Thu, 10 Oct 2024 18:40:35 +0300 Subject: [PATCH 1/5] Upgrade Spectra to v1.1.0 --- .../3rdparty/Spectra/DavidsonSymEigsSolver.h | 93 ++ gtsam/3rdparty/Spectra/GenEigsBase.h | 218 ++-- .../Spectra/GenEigsComplexShiftSolver.h | 88 +- .../3rdparty/Spectra/GenEigsRealShiftSolver.h | 58 +- gtsam/3rdparty/Spectra/GenEigsSolver.h | 59 +- gtsam/3rdparty/Spectra/HermEigsBase.h | 477 +++++++ gtsam/3rdparty/Spectra/HermEigsSolver.h | 146 +++ gtsam/3rdparty/Spectra/JDSymEigsBase.h | 190 +++ gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h | 177 ++- gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h | 350 ++++-- gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h | 217 ++-- gtsam/3rdparty/Spectra/LinAlg/Lanczos.h | 136 +- .../Spectra/LinAlg/Orthogonalization.h | 141 +++ gtsam/3rdparty/Spectra/LinAlg/RitzPairs.h | 130 ++ gtsam/3rdparty/Spectra/LinAlg/SearchSpace.h | 96 ++ gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h | 77 +- .../Spectra/LinAlg/UpperHessenbergEigen.h | 45 +- .../Spectra/LinAlg/UpperHessenbergQR.h | 439 ++++--- .../Spectra/LinAlg/UpperHessenbergSchur.h | 450 +++++++ gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h | 55 +- .../Spectra/MatOp/DenseGenComplexShiftSolve.h | 54 +- .../3rdparty/Spectra/MatOp/DenseGenMatProd.h | 58 +- .../Spectra/MatOp/DenseGenRealShiftSolve.h | 42 +- .../3rdparty/Spectra/MatOp/DenseHermMatProd.h | 92 ++ .../3rdparty/Spectra/MatOp/DenseSymMatProd.h | 60 +- .../Spectra/MatOp/DenseSymShiftSolve.h | 50 +- gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h | 51 +- .../MatOp/SparseGenComplexShiftSolve.h | 124 ++ .../3rdparty/Spectra/MatOp/SparseGenMatProd.h | 61 +- .../Spectra/MatOp/SparseGenRealShiftSolve.h | 45 +- .../Spectra/MatOp/SparseHermMatProd.h | 92 ++ .../Spectra/MatOp/SparseRegularInverse.h | 63 +- .../3rdparty/Spectra/MatOp/SparseSymMatProd.h | 63 +- .../Spectra/MatOp/SparseSymShiftSolve.h | 47 +- gtsam/3rdparty/Spectra/MatOp/SymShiftInvert.h | 245 ++++ .../Spectra/MatOp/internal/ArnoldiOp.h | 86 +- .../MatOp/internal/SymGEigsBucklingOp.h | 95 ++ .../Spectra/MatOp/internal/SymGEigsCayleyOp.h | 105 ++ .../MatOp/internal/SymGEigsCholeskyOp.h | 46 +- .../Spectra/MatOp/internal/SymGEigsRegInvOp.h | 46 +- .../MatOp/internal/SymGEigsShiftInvertOp.h | 95 ++ gtsam/3rdparty/Spectra/SymEigsBase.h | 448 ------- gtsam/3rdparty/Spectra/SymEigsShiftSolver.h | 85 +- gtsam/3rdparty/Spectra/SymEigsSolver.h | 73 +- gtsam/3rdparty/Spectra/SymGEigsShiftSolver.h | 463 +++++++ gtsam/3rdparty/Spectra/SymGEigsSolver.h | 200 ++- gtsam/3rdparty/Spectra/Util/CompInfo.h | 29 +- gtsam/3rdparty/Spectra/Util/GEigsMode.h | 24 +- gtsam/3rdparty/Spectra/Util/SelectionRule.h | 259 ++-- gtsam/3rdparty/Spectra/Util/SimpleRandom.h | 120 +- gtsam/3rdparty/Spectra/Util/TypeTraits.h | 52 +- gtsam/3rdparty/Spectra/Util/Version.h | 16 + gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h | 1103 ++++++++--------- .../Spectra/contrib/PartialSVDSolver.h | 99 +- 54 files changed, 5861 insertions(+), 2372 deletions(-) create mode 100644 gtsam/3rdparty/Spectra/DavidsonSymEigsSolver.h create mode 100644 gtsam/3rdparty/Spectra/HermEigsBase.h create mode 100644 gtsam/3rdparty/Spectra/HermEigsSolver.h create mode 100644 gtsam/3rdparty/Spectra/JDSymEigsBase.h create mode 100644 gtsam/3rdparty/Spectra/LinAlg/Orthogonalization.h create mode 100644 gtsam/3rdparty/Spectra/LinAlg/RitzPairs.h create mode 100644 gtsam/3rdparty/Spectra/LinAlg/SearchSpace.h create mode 100644 gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergSchur.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/DenseHermMatProd.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/SparseGenComplexShiftSolve.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/SparseHermMatProd.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/SymShiftInvert.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsBucklingOp.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCayleyOp.h create mode 100644 gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsShiftInvertOp.h delete mode 100644 gtsam/3rdparty/Spectra/SymEigsBase.h create mode 100644 gtsam/3rdparty/Spectra/SymGEigsShiftSolver.h create mode 100644 gtsam/3rdparty/Spectra/Util/Version.h diff --git a/gtsam/3rdparty/Spectra/DavidsonSymEigsSolver.h b/gtsam/3rdparty/Spectra/DavidsonSymEigsSolver.h new file mode 100644 index 0000000000..fe9f56c6d6 --- /dev/null +++ b/gtsam/3rdparty/Spectra/DavidsonSymEigsSolver.h @@ -0,0 +1,93 @@ +// Copyright (C) 2020 Netherlands eScience Center +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H +#define SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H + +#include + +#include "JDSymEigsBase.h" +#include "Util/SelectionRule.h" + +namespace Spectra { + +/// +/// \ingroup EigenSolver +/// +/// This class implement the DPR correction for the Davidson algorithms. +/// The algorithms in the Davidson family only differ in how the correction +/// vectors are computed and optionally in the initial orthogonal basis set. +/// +/// the DPR correction compute the new correction vector using the following expression: +/// \f[ correction = -(\boldsymbol{D} - \rho \boldsymbol{I})^{-1} \boldsymbol{r} \f] +/// where +/// \f$D\f$ is the diagonal of the target matrix, \f$\rho\f$ the Ritz eigenvalue, +/// \f$I\f$ the identity matrix and \f$r\f$ the residue vector. +/// +template +class DavidsonSymEigsSolver : public JDSymEigsBase, OpType> +{ +private: + using Index = Eigen::Index; + using Scalar = typename OpType::Scalar; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + + Vector m_diagonal; + +public: + DavidsonSymEigsSolver(OpType& op, Index nev, Index nvec_init, Index nvec_max) : + JDSymEigsBase, OpType>(op, nev, nvec_init, nvec_max) + { + m_diagonal.resize(this->m_matrix_operator.rows()); + for (Index i = 0; i < op.rows(); i++) + { + m_diagonal(i) = op(i, i); + } + } + + DavidsonSymEigsSolver(OpType& op, Index nev) : + DavidsonSymEigsSolver(op, nev, 2 * nev, 10 * nev) {} + + /// Create initial search space based on the diagonal + /// and the spectrum'target (highest or lowest) + /// + /// \param selection Spectrum section to target (e.g. lowest, etc.) + /// \return Matrix with the initial orthonormal basis + Matrix setup_initial_search_space(SortRule selection) const + { + std::vector indices_sorted = argsort(selection, m_diagonal); + + Matrix initial_basis = Matrix::Zero(this->m_matrix_operator.rows(), this->m_initial_search_space_size); + + for (Index k = 0; k < this->m_initial_search_space_size; k++) + { + Index row = indices_sorted[k]; + initial_basis(row, k) = 1.0; + } + return initial_basis; + } + + /// Compute the corrections using the DPR method. + /// + /// \return New correction vectors. + Matrix calculate_correction_vector() const + { + const Matrix& residues = this->m_ritz_pairs.residues(); + const Vector& eigvals = this->m_ritz_pairs.ritz_values(); + Matrix correction = Matrix::Zero(this->m_matrix_operator.rows(), this->m_correction_size); + for (Index k = 0; k < this->m_correction_size; k++) + { + Vector tmp = eigvals(k) - m_diagonal.array(); + correction.col(k) = residues.col(k).array() / tmp.array(); + } + return correction; + } +}; + +} // namespace Spectra + +#endif // SPECTRA_DAVIDSON_SYM_EIGS_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/GenEigsBase.h b/gtsam/3rdparty/Spectra/GenEigsBase.h index e084033d79..f466c4caec 100644 --- a/gtsam/3rdparty/Spectra/GenEigsBase.h +++ b/gtsam/3rdparty/Spectra/GenEigsBase.h @@ -1,11 +1,11 @@ -// Copyright (C) 2018-2019 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef GEN_EIGS_BASE_H -#define GEN_EIGS_BASE_H +#ifndef SPECTRA_GEN_EIGS_BASE_H +#define SPECTRA_GEN_EIGS_BASE_H #include #include // std::vector @@ -14,6 +14,7 @@ #include // std::complex, std::conj, std::norm, std::abs #include // std::invalid_argument +#include "Util/Version.h" #include "Util/TypeTraits.h" #include "Util/SelectionRule.h" #include "Util/CompInfo.h" @@ -33,32 +34,30 @@ namespace Spectra { /// It is kept here to provide the documentation for member functions of concrete eigen solvers /// such as GenEigsSolver and GenEigsRealShiftSolver. /// -template +template class GenEigsBase { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Array Array; - typedef Eigen::Array BoolArray; - typedef Eigen::Map MapMat; - typedef Eigen::Map MapVec; - typedef Eigen::Map MapConstVec; - - typedef std::complex Complex; - typedef Eigen::Matrix ComplexMatrix; - typedef Eigen::Matrix ComplexVector; - - typedef ArnoldiOp ArnoldiOpType; - typedef Arnoldi ArnoldiFac; + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using Array = Eigen::Array; + using BoolArray = Eigen::Array; + using MapMat = Eigen::Map; + using MapVec = Eigen::Map; + using MapConstVec = Eigen::Map; + + using Complex = std::complex; + using ComplexMatrix = Eigen::Matrix; + using ComplexVector = Eigen::Matrix; + + using ArnoldiOpType = ArnoldiOp; + using ArnoldiFac = Arnoldi; protected: // clang-format off - OpType* m_op; // object to conduct matrix operation, + OpType& m_op; // object to conduct matrix operation, // e.g. matrix-vector product const Index m_n; // dimension of matrix A const Index m_nev; // number of eigenvalues requested @@ -70,16 +69,11 @@ class GenEigsBase ComplexVector m_ritz_val; // Ritz values ComplexMatrix m_ritz_vec; // Ritz vectors - ComplexVector m_ritz_est; // last row of m_ritz_vec + ComplexVector m_ritz_est; // last row of m_ritz_vec, also called the Ritz estimates private: BoolArray m_ritz_conv; // indicator of the convergence of Ritz values - int m_info; // status of the computation - - const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow - // ~= 1e-307 for the "double" type - const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type - const Scalar m_eps23; // m_eps^(2/3), used to test the convergence + CompInfo m_info; // status of the computation // clang-format on // Real Ritz values calculated from UpperHessenbergEigen have exact zero imaginary part @@ -89,7 +83,7 @@ class GenEigsBase static bool is_conj(const Complex& v1, const Complex& v2) { return v1 == Eigen::numext::conj(v2); } // Implicitly restarted Arnoldi factorization - void restart(Index k) + void restart(Index k, SortRule selection) { using std::norm; @@ -140,19 +134,27 @@ class GenEigsBase m_fac.compress_V(Q); m_fac.factorize_from(k, m_ncv, m_nmatop); - retrieve_ritzpair(); + retrieve_ritzpair(selection); } // Calculates the number of converged Ritz values - Index num_converged(Scalar tol) + Index num_converged(const Scalar& tol) { - // thresh = tol * max(m_eps23, abs(theta)), theta for Ritz value - Array thresh = tol * m_ritz_val.head(m_nev).array().abs().max(m_eps23); + using std::pow; + + // The machine precision, ~= 1e-16 for the "double" type + const Scalar eps = TypeTraits::epsilon(); + // std::pow() is not constexpr, so we do not declare eps23 to be constexpr + // But most compilers should be able to compute eps23 at compile time + const Scalar eps23 = pow(eps, Scalar(2) / 3); + + // thresh = tol * max(eps23, abs(theta)), theta for Ritz value + Array thresh = tol * m_ritz_val.head(m_nev).array().abs().max(eps23); Array resid = m_ritz_est.head(m_nev).array().abs() * m_fac.f_norm(); // Converged "wanted" Ritz values m_ritz_conv = (resid < thresh); - return m_ritz_conv.cast().sum(); + return m_ritz_conv.count(); } // Returns the adjusted nev for restarting @@ -160,13 +162,17 @@ class GenEigsBase { using std::abs; + // A very small value, but 1.0 / near_0 does not overflow + // ~= 1e-307 for the "double" type + const Scalar near_0 = TypeTraits::min() * Scalar(10); + Index nev_new = m_nev; for (Index i = m_nev; i < m_ncv; i++) - if (abs(m_ritz_est[i]) < m_near_0) + if (abs(m_ritz_est[i]) < near_0) nev_new++; // Adjust nev_new, according to dnaup2.f line 660~674 in ARPACK - nev_new += std::min(nconv, (m_ncv - nev_new) / 2); + nev_new += (std::min)(nconv, (m_ncv - nev_new) / 2); if (nev_new == 1 && m_ncv >= 6) nev_new = m_ncv / 2; else if (nev_new == 1 && m_ncv > 3) @@ -187,14 +193,55 @@ class GenEigsBase } // Retrieves and sorts Ritz values and Ritz vectors - void retrieve_ritzpair() + void retrieve_ritzpair(SortRule selection) { UpperHessenbergEigen decomp(m_fac.matrix_H()); const ComplexVector& evals = decomp.eigenvalues(); ComplexMatrix evecs = decomp.eigenvectors(); - SortEigenvalue sorting(evals.data(), evals.size()); - std::vector ind = sorting.index(); + // Sort Ritz values and put the wanted ones at the beginning + std::vector ind; + switch (selection) + { + case SortRule::LargestMagn: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + case SortRule::LargestReal: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + case SortRule::LargestImag: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + case SortRule::SmallestMagn: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + case SortRule::SmallestReal: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + case SortRule::SmallestImag: + { + SortEigenvalue sorting(evals.data(), m_ncv); + sorting.swap(ind); + break; + } + default: + throw std::invalid_argument("unsupported selection rule"); + } // Copy the Ritz values and vectors to m_ritz_val and m_ritz_vec, respectively for (Index i = 0; i < m_ncv; i++) @@ -211,44 +258,45 @@ class GenEigsBase protected: // Sorts the first nev Ritz pairs in the specified order // This is used to return the final results - virtual void sort_ritzpair(int sort_rule) + virtual void sort_ritzpair(SortRule sort_rule) { - // First make sure that we have a valid index vector - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - std::vector ind = sorting.index(); - + std::vector ind; switch (sort_rule) { - case LARGEST_MAGN: + case SortRule::LargestMagn: + { + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; - case LARGEST_REAL: + } + case SortRule::LargestReal: { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; } - case LARGEST_IMAG: + case SortRule::LargestImag: { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; } - case SMALLEST_MAGN: + case SortRule::SmallestMagn: { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; } - case SMALLEST_REAL: + case SortRule::SmallestReal: { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; } - case SMALLEST_IMAG: + case SortRule::SmallestImag: { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); + SortEigenvalue sorting(m_ritz_val.data(), m_nev); + sorting.swap(ind); break; } default: @@ -274,18 +322,15 @@ class GenEigsBase public: /// \cond - GenEigsBase(OpType* op, BOpType* Bop, Index nev, Index ncv) : + GenEigsBase(OpType& op, const BOpType& Bop, Index nev, Index ncv) : m_op(op), - m_n(m_op->rows()), + m_n(m_op.rows()), m_nev(nev), m_ncv(ncv > m_n ? m_n : ncv), m_nmatop(0), m_niter(0), m_fac(ArnoldiOpType(op, Bop), m_ncv), - m_info(NOT_COMPUTED), - m_near_0(TypeTraits::min() * Scalar(10)), - m_eps(Eigen::NumTraits::epsilon()), - m_eps23(Eigen::numext::pow(m_eps, Scalar(2.0) / 3)) + m_info(CompInfo::NotComputed) { if (nev < 1 || nev > m_n - 2) throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 2, n is the size of matrix"); @@ -348,28 +393,33 @@ class GenEigsBase /// /// Conducts the major computation procedure. /// + /// \param selection An enumeration value indicating the selection rule of + /// the requested eigenvalues, for example `SortRule::LargestMagn` + /// to retrieve eigenvalues with the largest magnitude. + /// The full list of enumeration values can be found in + /// \ref Enumerations. /// \param maxit Maximum number of iterations allowed in the algorithm. /// \param tol Precision parameter for the calculated eigenvalues. - /// \param sort_rule Rule to sort the eigenvalues and eigenvectors. + /// \param sorting Rule to sort the eigenvalues and eigenvectors. /// Supported values are - /// `Spectra::LARGEST_MAGN`, `Spectra::LARGEST_REAL`, - /// `Spectra::LARGEST_IMAG`, `Spectra::SMALLEST_MAGN`, - /// `Spectra::SMALLEST_REAL` and `Spectra::SMALLEST_IMAG`, - /// for example `LARGEST_MAGN` indicates that eigenvalues + /// `SortRule::LargestMagn`, `SortRule::LargestReal`, + /// `SortRule::LargestImag`, `SortRule::SmallestMagn`, + /// `SortRule::SmallestReal` and `SortRule::SmallestImag`, + /// for example `SortRule::LargestMagn` indicates that eigenvalues /// with largest magnitude come first. /// Note that this argument is only used to /// **sort** the final result, and the **selection** rule /// (e.g. selecting the largest or smallest eigenvalues in the - /// full spectrum) is specified by the template parameter - /// `SelectionRule` of GenEigsSolver. + /// full spectrum) is specified by the parameter `selection`. /// /// \return Number of converged eigenvalues. /// - Index compute(Index maxit = 1000, Scalar tol = 1e-10, int sort_rule = LARGEST_MAGN) + Index compute(SortRule selection = SortRule::LargestMagn, Index maxit = 1000, + Scalar tol = 1e-10, SortRule sorting = SortRule::LargestMagn) { // The m-step Arnoldi factorization m_fac.factorize_from(1, m_ncv, m_nmatop); - retrieve_ritzpair(); + retrieve_ritzpair(selection); // Restarting Index i, nconv = 0, nev_adj; for (i = 0; i < maxit; i++) @@ -379,22 +429,22 @@ class GenEigsBase break; nev_adj = nev_adjusted(nconv); - restart(nev_adj); + restart(nev_adj, selection); } // Sorting results - sort_ritzpair(sort_rule); + sort_ritzpair(sorting); m_niter += i + 1; - m_info = (nconv >= m_nev) ? SUCCESSFUL : NOT_CONVERGING; + m_info = (nconv >= m_nev) ? CompInfo::Successful : CompInfo::NotConverging; - return std::min(m_nev, nconv); + return (std::min)(m_nev, nconv); } /// /// Returns the status of the computation. /// The full list of enumeration values can be found in \ref Enumerations. /// - int info() const { return m_info; } + CompInfo info() const { return m_info; } /// /// Returns the number of iterations used in the computation. @@ -446,7 +496,7 @@ class GenEigsBase ComplexMatrix eigenvectors(Index nvec) const { const Index nconv = m_ritz_conv.cast().sum(); - nvec = std::min(nvec, nconv); + nvec = (std::min)(nvec, nconv); ComplexMatrix res(m_n, nvec); if (!nvec) @@ -479,4 +529,4 @@ class GenEigsBase } // namespace Spectra -#endif // GEN_EIGS_BASE_H +#endif // SPECTRA_GEN_EIGS_BASE_H diff --git a/gtsam/3rdparty/Spectra/GenEigsComplexShiftSolver.h b/gtsam/3rdparty/Spectra/GenEigsComplexShiftSolver.h index da7c1b422e..2212669906 100644 --- a/gtsam/3rdparty/Spectra/GenEigsComplexShiftSolver.h +++ b/gtsam/3rdparty/Spectra/GenEigsComplexShiftSolver.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef GEN_EIGS_COMPLEX_SHIFT_SOLVER_H -#define GEN_EIGS_COMPLEX_SHIFT_SOLVER_H +#ifndef SPECTRA_GEN_EIGS_COMPLEX_SHIFT_SOLVER_H +#define SPECTRA_GEN_EIGS_COMPLEX_SHIFT_SOLVER_H #include @@ -23,33 +23,35 @@ namespace Spectra { /// knowledge of the shift-and-invert mode can be found in the documentation /// of the SymEigsShiftSolver class. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the shifted-and-inverted eigenvalues. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class. Users could either -/// use the DenseGenComplexShiftSolve wrapper class, or define their -/// own that implements all the public member functions as in -/// DenseGenComplexShiftSolve. +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseGenComplexShiftSolve and +/// SparseGenComplexShiftSolve, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseGenComplexShiftSolve. /// -template > -class GenEigsComplexShiftSolver : public GenEigsBase +template > +class GenEigsComplexShiftSolver : public GenEigsBase { private: - typedef Eigen::Index Index; - typedef std::complex Complex; - typedef Eigen::Matrix Vector; - typedef Eigen::Matrix ComplexVector; + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Complex = std::complex; + using Vector = Eigen::Matrix; + using ComplexArray = Eigen::Array; + + using Base = GenEigsBase; + using Base::m_op; + using Base::m_n; + using Base::m_nev; + using Base::m_fac; + using Base::m_ritz_val; + using Base::m_ritz_vec; const Scalar m_sigmar; const Scalar m_sigmai; // First transform back the Ritz values, and then sort - void sort_ritzpair(int sort_rule) + void sort_ritzpair(SortRule sort_rule) override { using std::abs; using std::sqrt; @@ -77,20 +79,20 @@ class GenEigsComplexShiftSolver : public GenEigsBase rng(0); const Scalar shiftr = rng.random() * m_sigmar + rng.random(); const Complex shift = Complex(shiftr, Scalar(0)); - this->m_op->set_shift(shiftr, Scalar(0)); + m_op.set_shift(shiftr, Scalar(0)); // Calculate inv(A - r * I) * vj - Vector v_real(this->m_n), v_imag(this->m_n), OPv_real(this->m_n), OPv_imag(this->m_n); - const Scalar eps = Eigen::NumTraits::epsilon(); - for (Index i = 0; i < this->m_nev; i++) + Vector v_real(m_n), v_imag(m_n), OPv_real(m_n), OPv_imag(m_n); + const Scalar eps = TypeTraits::epsilon(); + for (Index i = 0; i < m_nev; i++) { - v_real.noalias() = this->m_fac.matrix_V() * this->m_ritz_vec.col(i).real(); - v_imag.noalias() = this->m_fac.matrix_V() * this->m_ritz_vec.col(i).imag(); - this->m_op->perform_op(v_real.data(), OPv_real.data()); - this->m_op->perform_op(v_imag.data(), OPv_imag.data()); + v_real.noalias() = m_fac.matrix_V() * m_ritz_vec.col(i).real(); + v_imag.noalias() = m_fac.matrix_V() * m_ritz_vec.col(i).imag(); + m_op.perform_op(v_real.data(), OPv_real.data()); + m_op.perform_op(v_imag.data(), OPv_imag.data()); // Two roots computed from the quadratic equation - const Complex nu = this->m_ritz_val[i]; + const Complex nu = m_ritz_val[i]; const Complex root_part1 = m_sigmar + Scalar(0.5) / nu; const Complex root_part2 = Scalar(0.5) * sqrt(Scalar(1) - Scalar(4) * m_sigmai * m_sigmai * (nu * nu)) / nu; const Complex root1 = root_part1 + root_part2; @@ -98,7 +100,7 @@ class GenEigsComplexShiftSolver : public GenEigsBasem_n; k++) + for (int k = 0; k < m_n; k++) { const Complex rhs1 = Complex(v_real[k], v_imag[k]) / (root1 - shift); const Complex rhs2 = Complex(v_real[k], v_imag[k]) / (root2 - shift); @@ -108,31 +110,31 @@ class GenEigsComplexShiftSolver : public GenEigsBasem_ritz_val[i] = lambdaj; + m_ritz_val[i] = lambdaj; if (abs(Eigen::numext::imag(lambdaj)) > eps) { - this->m_ritz_val[i + 1] = Eigen::numext::conj(lambdaj); + m_ritz_val[i + 1] = Eigen::numext::conj(lambdaj); i++; } else { - this->m_ritz_val[i] = Complex(Eigen::numext::real(lambdaj), Scalar(0)); + m_ritz_val[i] = Complex(Eigen::numext::real(lambdaj), Scalar(0)); } } - GenEigsBase::sort_ritzpair(sort_rule); + Base::sort_ritzpair(sort_rule); } public: /// /// Constructor to create a eigen solver object using the shift-and-invert mode. /// - /// \param op Pointer to the matrix operation object. This class should implement + /// \param op The matrix operation object that implements /// the complex shift-solve operation of \f$A\f$: calculating /// \f$\mathrm{Re}\{(A-\sigma I)^{-1}v\}\f$ for any vector \f$v\f$. Users could either - /// create the object from the DenseGenComplexShiftSolve wrapper class, or - /// define their own that implements all the public member functions + /// create the object from the wrapper class such as DenseGenComplexShiftSolve, or + /// define their own that implements all the public members /// as in DenseGenComplexShiftSolve. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$, /// where \f$n\f$ is the size of matrix. @@ -144,14 +146,14 @@ class GenEigsComplexShiftSolver : public GenEigsBase(op, NULL, nev, ncv), + GenEigsComplexShiftSolver(OpType& op, Index nev, Index ncv, const Scalar& sigmar, const Scalar& sigmai) : + Base(op, IdentityBOp(), nev, ncv), m_sigmar(sigmar), m_sigmai(sigmai) { - this->m_op->set_shift(m_sigmar, m_sigmai); + op.set_shift(m_sigmar, m_sigmai); } }; } // namespace Spectra -#endif // GEN_EIGS_COMPLEX_SHIFT_SOLVER_H +#endif // SPECTRA_GEN_EIGS_COMPLEX_SHIFT_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/GenEigsRealShiftSolver.h b/gtsam/3rdparty/Spectra/GenEigsRealShiftSolver.h index 6f28308f1f..6e694e674a 100644 --- a/gtsam/3rdparty/Spectra/GenEigsRealShiftSolver.h +++ b/gtsam/3rdparty/Spectra/GenEigsRealShiftSolver.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef GEN_EIGS_REAL_SHIFT_SOLVER_H -#define GEN_EIGS_REAL_SHIFT_SOLVER_H +#ifndef SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H +#define SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H #include @@ -23,49 +23,45 @@ namespace Spectra { /// knowledge of the shift-and-invert mode can be found in the documentation /// of the SymEigsShiftSolver class. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the shifted-and-inverted eigenvalues. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class. Users could either -/// use the wrapper classes such as DenseGenRealShiftSolve and -/// SparseGenRealShiftSolve, or define their -/// own that implements all the public member functions as in -/// DenseGenRealShiftSolve. +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseGenRealShiftSolve and +/// SparseGenRealShiftSolve, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseGenRealShiftSolve. /// -template > -class GenEigsRealShiftSolver : public GenEigsBase +template > +class GenEigsRealShiftSolver : public GenEigsBase { private: - typedef Eigen::Index Index; - typedef std::complex Complex; - typedef Eigen::Array ComplexArray; + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Complex = std::complex; + using ComplexArray = Eigen::Array; + + using Base = GenEigsBase; + using Base::m_nev; + using Base::m_ritz_val; const Scalar m_sigma; // First transform back the Ritz values, and then sort - void sort_ritzpair(int sort_rule) + void sort_ritzpair(SortRule sort_rule) override { // The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma) // So the eigenvalues of the original problem is lambda = 1 / nu + sigma - ComplexArray ritz_val_org = Scalar(1.0) / this->m_ritz_val.head(this->m_nev).array() + m_sigma; - this->m_ritz_val.head(this->m_nev) = ritz_val_org; - GenEigsBase::sort_ritzpair(sort_rule); + m_ritz_val.head(m_nev) = Scalar(1) / m_ritz_val.head(m_nev).array() + m_sigma; + Base::sort_ritzpair(sort_rule); } public: /// /// Constructor to create a eigen solver object using the shift-and-invert mode. /// - /// \param op Pointer to the matrix operation object. This class should implement + /// \param op The matrix operation object that implements /// the shift-solve operation of \f$A\f$: calculating /// \f$(A-\sigma I)^{-1}v\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper class such as DenseGenRealShiftSolve, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseGenRealShiftSolve. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$, /// where \f$n\f$ is the size of matrix. @@ -76,14 +72,14 @@ class GenEigsRealShiftSolver : public GenEigsBase(op, NULL, nev, ncv), + GenEigsRealShiftSolver(OpType& op, Index nev, Index ncv, const Scalar& sigma) : + Base(op, IdentityBOp(), nev, ncv), m_sigma(sigma) { - this->m_op->set_shift(m_sigma); + op.set_shift(m_sigma); } }; } // namespace Spectra -#endif // GEN_EIGS_REAL_SHIFT_SOLVER_H +#endif // SPECTRA_GEN_EIGS_REAL_SHIFT_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/GenEigsSolver.h b/gtsam/3rdparty/Spectra/GenEigsSolver.h index 3ead1dc4d8..4e63290219 100644 --- a/gtsam/3rdparty/Spectra/GenEigsSolver.h +++ b/gtsam/3rdparty/Spectra/GenEigsSolver.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef GEN_EIGS_SOLVER_H -#define GEN_EIGS_SOLVER_H +#ifndef SPECTRA_GEN_EIGS_SOLVER_H +#define SPECTRA_GEN_EIGS_SOLVER_H #include @@ -25,18 +25,11 @@ namespace Spectra { /// also applies to the GenEigsSolver class here, except that the eigenvalues /// and eigenvectors of a general matrix can now be complex-valued. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the requested eigenvalues, for example `LARGEST_MAGN` -/// to retrieve eigenvalues with the largest magnitude. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class. Users could either -/// use the wrapper classes such as DenseGenMatProd and -/// SparseGenMatProd, or define their -/// own that implements all the public member functions as in -/// DenseGenMatProd. +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseGenMatProd and +/// SparseGenMatProd, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseGenMatProd. /// /// An example that illustrates the usage of GenEigsSolver is give below: /// @@ -58,15 +51,15 @@ namespace Spectra { /// /// // Construct eigen solver object, requesting the largest /// // (in magnitude, or norm) three eigenvalues -/// GenEigsSolver< double, LARGEST_MAGN, DenseGenMatProd > eigs(&op, 3, 6); +/// GenEigsSolver> eigs(op, 3, 6); /// /// // Initialize and compute /// eigs.init(); -/// int nconv = eigs.compute(); +/// int nconv = eigs.compute(SortRule::LargestMagn); /// /// // Retrieve results /// Eigen::VectorXcd evalues; -/// if(eigs.info() == SUCCESSFUL) +/// if (eigs.info() == CompInfo::Successful) /// evalues = eigs.eigenvalues(); /// /// std::cout << "Eigenvalues found:\n" << evalues << std::endl; @@ -93,12 +86,12 @@ namespace Spectra { /// const int n = 10; /// Eigen::SparseMatrix M(n, n); /// M.reserve(Eigen::VectorXi::Constant(n, 3)); -/// for(int i = 0; i < n; i++) +/// for (int i = 0; i < n; i++) /// { /// M.insert(i, i) = 1.0; -/// if(i > 0) +/// if (i > 0) /// M.insert(i - 1, i) = 3.0; -/// if(i < n - 1) +/// if (i < n - 1) /// M.insert(i + 1, i) = 2.0; /// } /// @@ -106,15 +99,15 @@ namespace Spectra { /// SparseGenMatProd op(M); /// /// // Construct eigen solver object, requesting the largest three eigenvalues -/// GenEigsSolver< double, LARGEST_MAGN, SparseGenMatProd > eigs(&op, 3, 6); +/// GenEigsSolver> eigs(op, 3, 6); /// /// // Initialize and compute /// eigs.init(); -/// int nconv = eigs.compute(); +/// int nconv = eigs.compute(SortRule::LargestMagn); /// /// // Retrieve results /// Eigen::VectorXcd evalues; -/// if(eigs.info() == SUCCESSFUL) +/// if (eigs.info() == CompInfo::Successful) /// evalues = eigs.eigenvalues(); /// /// std::cout << "Eigenvalues found:\n" << evalues << std::endl; @@ -122,23 +115,21 @@ namespace Spectra { /// return 0; /// } /// \endcode -template > -class GenEigsSolver : public GenEigsBase +template > +class GenEigsSolver : public GenEigsBase { private: - typedef Eigen::Index Index; + using Index = Eigen::Index; public: /// /// Constructor to create a solver object. /// - /// \param op Pointer to the matrix operation object, which should implement + /// \param op The matrix operation object that implements /// the matrix-vector multiplication operation of \f$A\f$: /// calculating \f$Av\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper class such as DenseGenMatProd, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseGenMatProd. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-2\f$, /// where \f$n\f$ is the size of matrix. @@ -148,11 +139,11 @@ class GenEigsSolver : public GenEigsBase(op, NULL, nev, ncv) + GenEigsSolver(OpType& op, Index nev, Index ncv) : + GenEigsBase(op, IdentityBOp(), nev, ncv) {} }; } // namespace Spectra -#endif // GEN_EIGS_SOLVER_H +#endif // SPECTRA_GEN_EIGS_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/HermEigsBase.h b/gtsam/3rdparty/Spectra/HermEigsBase.h new file mode 100644 index 0000000000..2426b5f6c3 --- /dev/null +++ b/gtsam/3rdparty/Spectra/HermEigsBase.h @@ -0,0 +1,477 @@ +// Copyright (C) 2018-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_HERM_EIGS_BASE_H +#define SPECTRA_HERM_EIGS_BASE_H + +#include +#include // std::vector +#include // std::abs, std::pow +#include // std::min +#include // std::invalid_argument +#include // std::move + +#include "Util/Version.h" +#include "Util/TypeTraits.h" +#include "Util/SelectionRule.h" +#include "Util/CompInfo.h" +#include "Util/SimpleRandom.h" +#include "MatOp/internal/ArnoldiOp.h" +#include "LinAlg/UpperHessenbergQR.h" +#include "LinAlg/TridiagEigen.h" +#include "LinAlg/Lanczos.h" + +namespace Spectra { + +/// +/// \defgroup EigenSolver Eigen Solvers +/// +/// Eigen solvers for different types of problems. +/// + +/// +/// \ingroup EigenSolver +/// +/// This is the base class for Hermitian (and real symmetric) eigen solvers, +/// mainly for internal use. +/// It is kept here to provide the documentation for member functions of +/// concrete eigen solvers such as SymEigsSolver, HermEigsSolver, SymEigsShiftSolver, etc. +/// +template +class HermEigsBase +{ +private: + using Scalar = typename OpType::Scalar; + // The real part type of the matrix element, e.g., + // Scalar = double => RealScalar = double + // Scalar = std::complex => RealScalar = double + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using RealMatrix = Eigen::Matrix; + using RealVector = Eigen::Matrix; + using RealArray = Eigen::Array; + using BoolArray = Eigen::Array; + using MapMat = Eigen::Map; + using MapVec = Eigen::Map; + using MapConstVec = Eigen::Map; + + using ArnoldiOpType = ArnoldiOp; + using LanczosFac = Lanczos; + +protected: + // clang-format off + + // In SymEigsSolver and SymEigsShiftSolver, the A operator is an lvalue provided by + // the user. In SymGEigsSolver, the A operator is an rvalue. To avoid copying objects, + // we use the following scheme: + // 1. If the op parameter in the constructor is an lvalue, make m_op a const reference to op + // 2. If op is an rvalue, move op to m_op_container, and then make m_op a const + // reference to m_op_container[0] + std::vector m_op_container; + const OpType& m_op; // matrix operator for A + const Index m_n; // dimension of matrix A + const Index m_nev; // number of eigenvalues requested + const Index m_ncv; // dimension of Krylov subspace in the Lanczos method + Index m_nmatop; // number of matrix operations called + Index m_niter; // number of restarting iterations + + LanczosFac m_fac; // Lanczos factorization + RealVector m_ritz_val; // Ritz values + +private: + RealMatrix m_ritz_vec; // Ritz vectors + RealVector m_ritz_est; // last row of m_ritz_vec, also called the Ritz estimates + BoolArray m_ritz_conv; // indicator of the convergence of Ritz values + CompInfo m_info; // status of the computation + // clang-format on + + // Move rvalue object to the container + static std::vector create_op_container(OpType&& rval) + { + std::vector container; + container.emplace_back(std::move(rval)); + return container; + } + + // Implicitly restarted Lanczos factorization + void restart(Index k, SortRule selection) + { + using std::abs; + + if (k >= m_ncv) + return; + + // QR decomposition on a real symmetric matrix + TridiagQR decomp(m_ncv); + // Q is a real orthogonal matrix + RealMatrix Q = RealMatrix::Identity(m_ncv, m_ncv); + + // Apply large shifts first + const int nshift = m_ncv - k; + RealVector shifts = m_ritz_val.tail(nshift); + std::sort(shifts.data(), shifts.data() + nshift, + [](const RealScalar& v1, const RealScalar& v2) { return abs(v1) > abs(v2); }); + + for (Index i = 0; i < nshift; i++) + { + // QR decomposition of H-mu*I, mu is the shift + // H is known to be a real symmetric matrix + decomp.compute(m_fac.matrix_H().real(), shifts[i]); + + // Q -> Q * Qi + decomp.apply_YQ(Q); + // H -> Q'HQ + // Since QR = H - mu * I, we have H = QR + mu * I + // and therefore Q'HQ = RQ + mu * I + m_fac.compress_H(decomp); + // Note that in our setting, mu is an eigenvalue of H, + // so after applying Q'HQ, H must have be of the following form + // H = [X 0 0] + // [0 mu 0] + // [0 0 D] + // Then we can force H[k, k-1] = H[k-1, k] = 0 and H[k, k] = mu, + // where k is the size of X + // + // Currently disabled due to numerical stability + // + // m_fac.deflate_H(m_ncv - i - 1, shifts[i]); + } + + m_fac.compress_V(Q); + m_fac.factorize_from(k, m_ncv, m_nmatop); + + retrieve_ritzpair(selection); + } + + // Calculates the number of converged Ritz values + Index num_converged(const RealScalar& tol) + { + using std::pow; + + // The machine precision, ~= 1e-16 for the "double" type + const RealScalar eps = TypeTraits::epsilon(); + // std::pow() is not constexpr, so we do not declare eps23 to be constexpr + // But most compilers should be able to compute eps23 at compile time + const RealScalar eps23 = pow(eps, RealScalar(2) / 3); + + // thresh = tol * max(eps23, abs(theta)), theta for Ritz value + RealArray thresh = tol * m_ritz_val.head(m_nev).array().abs().max(eps23); + RealArray resid = m_ritz_est.head(m_nev).array().abs() * m_fac.f_norm(); + // Converged "wanted" Ritz values + m_ritz_conv = (resid < thresh); + + return m_ritz_conv.count(); + } + + // Returns the adjusted nev for restarting + Index nev_adjusted(Index nconv) + { + using std::abs; + + // A very small value, but 1.0 / near_0 does not overflow + // ~= 1e-307 for the "double" type + const RealScalar near_0 = TypeTraits::min() * RealScalar(10); + + Index nev_new = m_nev; + for (Index i = m_nev; i < m_ncv; i++) + if (abs(m_ritz_est[i]) < near_0) + nev_new++; + + // Adjust nev_new, according to dsaup2.f line 677~684 in ARPACK + nev_new += (std::min)(nconv, (m_ncv - nev_new) / 2); + if (nev_new == 1 && m_ncv >= 6) + nev_new = m_ncv / 2; + else if (nev_new == 1 && m_ncv > 2) + nev_new = 2; + + if (nev_new > m_ncv - 1) + nev_new = m_ncv - 1; + + return nev_new; + } + + // Retrieves and sorts Ritz values and Ritz vectors + void retrieve_ritzpair(SortRule selection) + { + TridiagEigen decomp(m_fac.matrix_H().real()); + const RealVector& evals = decomp.eigenvalues(); + const RealMatrix& evecs = decomp.eigenvectors(); + + // Sort Ritz values and put the wanted ones at the beginning + std::vector ind = argsort(selection, evals, m_ncv); + + // Copy the Ritz values and vectors to m_ritz_val and m_ritz_vec, respectively + for (Index i = 0; i < m_ncv; i++) + { + m_ritz_val[i] = evals[ind[i]]; + m_ritz_est[i] = evecs(m_ncv - 1, ind[i]); + } + for (Index i = 0; i < m_nev; i++) + { + m_ritz_vec.col(i).noalias() = evecs.col(ind[i]); + } + } + +protected: + // Sorts the first nev Ritz pairs in the specified order + // This is used to return the final results + virtual void sort_ritzpair(SortRule sort_rule) + { + if ((sort_rule != SortRule::LargestAlge) && (sort_rule != SortRule::LargestMagn) && + (sort_rule != SortRule::SmallestAlge) && (sort_rule != SortRule::SmallestMagn)) + throw std::invalid_argument("unsupported sorting rule"); + + std::vector ind = argsort(sort_rule, m_ritz_val, m_nev); + + RealVector new_ritz_val(m_ncv); + RealMatrix new_ritz_vec(m_ncv, m_nev); + BoolArray new_ritz_conv(m_nev); + + for (Index i = 0; i < m_nev; i++) + { + new_ritz_val[i] = m_ritz_val[ind[i]]; + new_ritz_vec.col(i).noalias() = m_ritz_vec.col(ind[i]); + new_ritz_conv[i] = m_ritz_conv[ind[i]]; + } + + m_ritz_val.swap(new_ritz_val); + m_ritz_vec.swap(new_ritz_vec); + m_ritz_conv.swap(new_ritz_conv); + } + +public: + /// \cond + + // If op is an lvalue + HermEigsBase(OpType& op, const BOpType& Bop, Index nev, Index ncv) : + m_op(op), + m_n(op.rows()), + m_nev(nev), + m_ncv(ncv > m_n ? m_n : ncv), + m_nmatop(0), + m_niter(0), + m_fac(ArnoldiOpType(op, Bop), m_ncv), + m_info(CompInfo::NotComputed) + { + if (nev < 1 || nev > m_n - 1) + throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 1, n is the size of matrix"); + + if (ncv <= nev || ncv > m_n) + throw std::invalid_argument("ncv must satisfy nev < ncv <= n, n is the size of matrix"); + } + + // If op is an rvalue + HermEigsBase(OpType&& op, const BOpType& Bop, Index nev, Index ncv) : + m_op_container(create_op_container(std::move(op))), + m_op(m_op_container.front()), + m_n(m_op.rows()), + m_nev(nev), + m_ncv(ncv > m_n ? m_n : ncv), + m_nmatop(0), + m_niter(0), + m_fac(ArnoldiOpType(m_op, Bop), m_ncv), + m_info(CompInfo::NotComputed) + { + if (nev < 1 || nev > m_n - 1) + throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 1, n is the size of matrix"); + + if (ncv <= nev || ncv > m_n) + throw std::invalid_argument("ncv must satisfy nev < ncv <= n, n is the size of matrix"); + } + + /// + /// Virtual destructor + /// + virtual ~HermEigsBase() {} + + /// \endcond + + /// + /// Initializes the solver by providing an initial residual vector. + /// + /// \param init_resid Pointer to the initial residual vector. + /// + /// **Spectra** (and also **ARPACK**) uses an iterative algorithm + /// to find eigenvalues. This function allows the user to provide the initial + /// residual vector. + /// + void init(const Scalar* init_resid) + { + // Reset all matrices/vectors to zero + m_ritz_val.resize(m_ncv); + m_ritz_vec.resize(m_ncv, m_nev); + m_ritz_est.resize(m_ncv); + m_ritz_conv.resize(m_nev); + + m_ritz_val.setZero(); + m_ritz_vec.setZero(); + m_ritz_est.setZero(); + m_ritz_conv.setZero(); + + m_nmatop = 0; + m_niter = 0; + + // Initialize the Lanczos factorization + MapConstVec v0(init_resid, m_n); + m_fac.init(v0, m_nmatop); + } + + /// + /// Initializes the solver by providing a random initial residual vector. + /// + /// This overloaded function generates a random initial residual vector + /// (with a fixed random seed) for the algorithm. Elements in the vector + /// follow independent Uniform(-0.5, 0.5) distribution. + /// + void init() + { + SimpleRandom rng(0); + Vector init_resid = rng.random_vec(m_n); + init(init_resid.data()); + } + + /// + /// Conducts the major computation procedure. + /// + /// \param selection An enumeration value indicating the selection rule of + /// the requested eigenvalues, for example `SortRule::LargestMagn` + /// to retrieve eigenvalues with the largest magnitude. + /// The full list of enumeration values can be found in + /// \ref Enumerations. + /// \param maxit Maximum number of iterations allowed in the algorithm. + /// \param tol Precision parameter for the calculated eigenvalues. + /// \param sorting Rule to sort the eigenvalues and eigenvectors. + /// Supported values are + /// `SortRule::LargestAlge`, `SortRule::LargestMagn`, + /// `SortRule::SmallestAlge`, and `SortRule::SmallestMagn`. + /// For example, `SortRule::LargestAlge` indicates that largest eigenvalues + /// come first. Note that this argument is only used to + /// **sort** the final result, and the **selection** rule + /// (e.g. selecting the largest or smallest eigenvalues in the + /// full spectrum) is specified by the parameter `selection`. + /// + /// \return Number of converged eigenvalues. + /// + Index compute(SortRule selection = SortRule::LargestMagn, Index maxit = 1000, + RealScalar tol = 1e-10, SortRule sorting = SortRule::LargestAlge) + { + // The m-step Lanczos factorization + m_fac.factorize_from(1, m_ncv, m_nmatop); + retrieve_ritzpair(selection); + // Restarting + Index i, nconv = 0, nev_adj; + for (i = 0; i < maxit; i++) + { + nconv = num_converged(tol); + if (nconv >= m_nev) + break; + + nev_adj = nev_adjusted(nconv); + restart(nev_adj, selection); + } + // Sorting results + sort_ritzpair(sorting); + + m_niter += i + 1; + m_info = (nconv >= m_nev) ? CompInfo::Successful : CompInfo::NotConverging; + + return (std::min)(m_nev, nconv); + } + + /// + /// Returns the status of the computation. + /// The full list of enumeration values can be found in \ref Enumerations. + /// + CompInfo info() const { return m_info; } + + /// + /// Returns the number of iterations used in the computation. + /// + Index num_iterations() const { return m_niter; } + + /// + /// Returns the number of matrix operations used in the computation. + /// + Index num_operations() const { return m_nmatop; } + + /// + /// Returns the converged eigenvalues. + /// + /// \return A vector containing the real-valued eigenvalues. + /// Returned vector type will be `Eigen::Vector`, depending on + /// the `Scalar` type defined in the matrix operation class. + /// For example, if `Scalar` is `double` or `std::complex`, + /// then `RealScalar` would be `double`. + /// + RealVector eigenvalues() const + { + const Index nconv = m_ritz_conv.count(); + RealVector res(nconv); + + if (!nconv) + return res; + + Index j = 0; + for (Index i = 0; i < m_nev; i++) + { + if (m_ritz_conv[i]) + { + res[j] = m_ritz_val[i]; + j++; + } + } + + return res; + } + + /// + /// Returns the eigenvectors associated with the converged eigenvalues. + /// + /// \param nvec The number of eigenvectors to return. + /// + /// \return A matrix containing the eigenvectors. + /// Returned matrix type will be `Eigen::Matrix`, + /// depending on the `Scalar` type defined in the matrix operation class. + /// + virtual Matrix eigenvectors(Index nvec) const + { + const Index nconv = m_ritz_conv.count(); + nvec = (std::min)(nvec, nconv); + Matrix res(m_n, nvec); + + if (!nvec) + return res; + + RealMatrix ritz_vec_conv(m_ncv, nvec); + Index j = 0; + for (Index i = 0; i < m_nev && j < nvec; i++) + { + if (m_ritz_conv[i]) + { + ritz_vec_conv.col(j).noalias() = m_ritz_vec.col(i); + j++; + } + } + + res.noalias() = m_fac.matrix_V() * ritz_vec_conv; + + return res; + } + + /// + /// Returns all converged eigenvectors. + /// + virtual Matrix eigenvectors() const + { + return eigenvectors(m_nev); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_HERM_EIGS_BASE_H diff --git a/gtsam/3rdparty/Spectra/HermEigsSolver.h b/gtsam/3rdparty/Spectra/HermEigsSolver.h new file mode 100644 index 0000000000..2b243872aa --- /dev/null +++ b/gtsam/3rdparty/Spectra/HermEigsSolver.h @@ -0,0 +1,146 @@ +// Copyright (C) 2024-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_HERM_EIGS_SOLVER_H +#define SPECTRA_HERM_EIGS_SOLVER_H + +#include + +#include "HermEigsBase.h" +#include "Util/SelectionRule.h" +#include "MatOp/DenseHermMatProd.h" + +namespace Spectra { + +/// +/// \ingroup EigenSolver +/// +/// This class implements the eigen solver for Hermitian matrices, i.e., +/// to solve \f$Ax=\lambda x\f$ where \f$A\f$ is Hermitian. +/// An Hermitian matrix is a complex square matrix that is equal to its +/// own conjugate transpose. It is known that all Hermitian matrices have +/// real-valued eigenvalues. +/// +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseHermMatProd and +/// SparseHermMatProd, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseHermMatProd. +/// +/// Below is an example that demonstrates the usage of this class. +/// +/// \code{.cpp} +/// #include +/// #include +/// // is implicitly included +/// #include +/// +/// using namespace Spectra; +/// +/// int main() +/// { +/// // We are going to calculate the eigenvalues of M +/// Eigen::MatrixXcd A = Eigen::MatrixXcd::Random(10, 10); +/// Eigen::MatrixXcd M = A + A.adjoint(); +/// +/// // Construct matrix operation object using the wrapper class DenseHermMatProd +/// using OpType = DenseHermMatProd>; +/// OpType op(M); +/// +/// // Construct eigen solver object, requesting the largest three eigenvalues +/// HermEigsSolver eigs(op, 3, 6); +/// +/// // Initialize and compute +/// eigs.init(); +/// int nconv = eigs.compute(SortRule::LargestAlge); +/// +/// // Retrieve results +/// // Eigenvalues are real-valued, and eigenvectors are complex-valued +/// Eigen::VectorXd evalues; +/// if (eigs.info() == CompInfo::Successful) +/// evalues = eigs.eigenvalues(); +/// +/// std::cout << "Eigenvalues found:\n" << evalues << std::endl; +/// +/// return 0; +/// } +/// \endcode +/// +/// And here is an example for user-supplied matrix operation class. +/// +/// \code{.cpp} +/// #include +/// #include +/// #include +/// +/// using namespace Spectra; +/// +/// // M = diag(1+0i, 2+0i, ..., 10+0i) +/// class MyDiagonalTen +/// { +/// public: +/// using Scalar = std::complex; // A typedef named "Scalar" is required +/// int rows() const { return 10; } +/// int cols() const { return 10; } +/// // y_out = M * x_in +/// void perform_op(Scalar *x_in, Scalar *y_out) const +/// { +/// for (int i = 0; i < rows(); i++) +/// { +/// y_out[i] = x_in[i] * Scalar(i + 1, 0); +/// } +/// } +/// }; +/// +/// int main() +/// { +/// MyDiagonalTen op; +/// HermEigsSolver eigs(op, 3, 6); +/// eigs.init(); +/// eigs.compute(SortRule::LargestAlge); +/// if (eigs.info() == CompInfo::Successful) +/// { +/// Eigen::VectorXd evalues = eigs.eigenvalues(); +/// // Will get (10, 9, 8) +/// std::cout << "Eigenvalues found:\n" << evalues << std::endl; +/// } +/// +/// return 0; +/// } +/// \endcode +/// +template > +class HermEigsSolver : public HermEigsBase +{ +private: + using Index = Eigen::Index; + +public: + /// + /// Constructor to create a solver object. + /// + /// \param op The matrix operation object that implements + /// the matrix-vector multiplication operation of \f$A\f$: + /// calculating \f$Av\f$ for any vector \f$v\f$. Users could either + /// create the object from the wrapper class such as DenseHermMatProd, or + /// define their own that implements all the public members + /// as in DenseHermMatProd. + /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, + /// where \f$n\f$ is the size of matrix. + /// \param ncv Parameter that controls the convergence speed of the algorithm. + /// Typically a larger `ncv` means faster convergence, but it may + /// also result in greater memory use and more matrix operations + /// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$, + /// and is advised to take \f$ncv \ge 2\cdot nev\f$. + /// + HermEigsSolver(OpType& op, Index nev, Index ncv) : + HermEigsBase(op, IdentityBOp(), nev, ncv) + {} +}; + +} // namespace Spectra + +#endif // SPECTRA_HERM_EIGS_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/JDSymEigsBase.h b/gtsam/3rdparty/Spectra/JDSymEigsBase.h new file mode 100644 index 0000000000..dca7d08c36 --- /dev/null +++ b/gtsam/3rdparty/Spectra/JDSymEigsBase.h @@ -0,0 +1,190 @@ +// Copyright (C) 2020 Netherlands eScience Center +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_JD_SYM_EIGS_BASE_H +#define SPECTRA_JD_SYM_EIGS_BASE_H + +#include +#include // std::vector +#include // std::abs, std::pow +#include // std::min +#include // std::invalid_argument +#include + +#include "Util/SelectionRule.h" +#include "Util/CompInfo.h" +#include "LinAlg/SearchSpace.h" +#include "LinAlg/RitzPairs.h" + +namespace Spectra { + +/// +/// \ingroup EigenSolver +/// +/// This is the base class for symmetric JD eigen solvers, mainly for internal use. +/// It is kept here to provide the documentation for member functions of concrete eigen solvers +/// such as DavidsonSymEigsSolver. +/// +/// This class uses the CRTP method to call functions from the derived class. +/// +template +class JDSymEigsBase +{ +protected: + using Index = Eigen::Index; + using Scalar = typename OpType::Scalar; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + + const OpType& m_matrix_operator; // object to conduct matrix operation, + // e.g. matrix-vector product + Index niter_ = 0; + const Index m_number_eigenvalues; // number of eigenvalues requested + Index m_max_search_space_size; + Index m_initial_search_space_size; + Index m_correction_size; // how many correction vectors are added in each iteration + RitzPairs m_ritz_pairs; // Ritz eigen pair structure + SearchSpace m_search_space; // search space + +private: + CompInfo m_info = CompInfo::NotComputed; // status of the computation + + void check_argument() const + { + if (m_number_eigenvalues < 1 || m_number_eigenvalues > m_matrix_operator.cols() - 1) + throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 1, n is the size of matrix"); + } + + void initialize() + { + // TODO better input validation and checks + if (m_matrix_operator.cols() < m_max_search_space_size) + { + m_max_search_space_size = m_matrix_operator.cols(); + } + if (m_matrix_operator.cols() < m_initial_search_space_size + m_correction_size) + { + m_initial_search_space_size = m_matrix_operator.cols() / 3; + m_correction_size = m_matrix_operator.cols() / 3; + } + } + +public: + JDSymEigsBase(OpType& op, Index nev, Index nvec_init, Index nvec_max) : + m_matrix_operator(op), + m_number_eigenvalues(nev), + m_max_search_space_size(nvec_max < op.rows() ? nvec_max : 10 * m_number_eigenvalues), + m_initial_search_space_size(nvec_init < op.rows() ? nvec_init : 2 * m_number_eigenvalues), + m_correction_size(m_number_eigenvalues) + { + check_argument(); + initialize(); + } + + JDSymEigsBase(OpType& op, Index nev) : + JDSymEigsBase(op, nev, 2 * nev, 10 * nev) {} + + /// + /// Sets the Maxmium SearchspaceSize after which is deflated + /// + void set_max_search_space_size(Index max_search_space_size) + { + m_max_search_space_size = max_search_space_size; + } + /// + /// Sets how many correction vectors are added in each iteration + /// + void set_correction_size(Index correction_size) + { + m_correction_size = correction_size; + } + + /// + /// Sets the Initial SearchspaceSize for Ritz values + /// + void set_initial_search_space_size(Index initial_search_space_size) + { + m_initial_search_space_size = initial_search_space_size; + } + + /// + /// Virtual destructor + /// + virtual ~JDSymEigsBase() {} + + /// + /// Returns the status of the computation. + /// The full list of enumeration values can be found in \ref Enumerations. + /// + CompInfo info() const { return m_info; } + + /// + /// Returns the number of iterations used in the computation. + /// + Index num_iterations() const { return niter_; } + + Vector eigenvalues() const { return m_ritz_pairs.ritz_values().head(m_number_eigenvalues); } + + Matrix eigenvectors() const { return m_ritz_pairs.ritz_vectors().leftCols(m_number_eigenvalues); } + + Index compute(SortRule selection = SortRule::LargestMagn, Index maxit = 100, + Scalar tol = 100 * Eigen::NumTraits::dummy_precision()) + { + Derived& derived = static_cast(*this); + Matrix intial_space = derived.setup_initial_search_space(selection); + return compute_with_guess(intial_space, selection, maxit, tol); + } + + Index compute_with_guess(const Eigen::Ref& initial_space, + SortRule selection = SortRule::LargestMagn, + Index maxit = 100, + Scalar tol = 100 * Eigen::NumTraits::dummy_precision()) + + { + m_search_space.initialize_search_space(initial_space); + niter_ = 0; + for (niter_ = 0; niter_ < maxit; niter_++) + { + bool do_restart = (m_search_space.size() > m_max_search_space_size); + + if (do_restart) + { + m_search_space.restart(m_ritz_pairs, m_initial_search_space_size); + } + + m_search_space.update_operator_basis_product(m_matrix_operator); + + Eigen::ComputationInfo small_problem_info = m_ritz_pairs.compute_eigen_pairs(m_search_space); + if (small_problem_info != Eigen::ComputationInfo::Success) + { + m_info = CompInfo::NumericalIssue; + break; + } + m_ritz_pairs.sort(selection); + + bool converged = m_ritz_pairs.check_convergence(tol, m_number_eigenvalues); + if (converged) + { + m_info = CompInfo::Successful; + break; + } + else if (niter_ == maxit - 1) + { + m_info = CompInfo::NotConverging; + break; + } + Derived& derived = static_cast(*this); + Matrix corr_vect = derived.calculate_correction_vector(); + + m_search_space.extend_basis(corr_vect); + } + return (m_ritz_pairs.converged_eigenvalues()).template cast().head(m_number_eigenvalues).sum(); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_JD_SYM_EIGS_BASE_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h b/gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h index 730ba95564..ca566493a0 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h +++ b/gtsam/3rdparty/Spectra/LinAlg/Arnoldi.h @@ -1,16 +1,16 @@ -// Copyright (C) 2018-2019 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef ARNOLDI_H -#define ARNOLDI_H +#ifndef SPECTRA_ARNOLDI_H +#define SPECTRA_ARNOLDI_H #include #include // std::sqrt +#include // std::move #include // std::invalid_argument -#include // std::stringstream #include "../MatOp/internal/ArnoldiOp.h" #include "../Util/TypeTraits.h" @@ -31,92 +31,127 @@ template class Arnoldi { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapMat; - typedef Eigen::Map MapVec; - typedef Eigen::Map MapConstMat; - typedef Eigen::Map MapConstVec; + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapVec = Eigen::Map; + using MapConstMat = Eigen::Map; + using MapConstVec = Eigen::Map; protected: - // clang-format off - ArnoldiOpType m_op; // Operators for the Arnoldi factorization - - const Index m_n; // dimension of A - const Index m_m; // maximum dimension of subspace V - Index m_k; // current dimension of subspace V - - Matrix m_fac_V; // V matrix in the Arnoldi factorization - Matrix m_fac_H; // H matrix in the Arnoldi factorization - Vector m_fac_f; // residual in the Arnoldi factorization - Scalar m_beta; // ||f||, B-norm of f - - const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow - // ~= 1e-307 for the "double" type - const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type - // clang-format on - - // Given orthonormal basis functions V, find a nonzero vector f such that V'Bf = 0 + // A very small value, but 1.0 / m_near_0 does not overflow + // ~= 1e-307 for the "double" type + const RealScalar m_near_0 = TypeTraits::min() * RealScalar(10); + // The machine precision, ~= 1e-16 for the "double" type + const RealScalar m_eps = TypeTraits::epsilon(); + + ArnoldiOpType m_op; // Operators for the Arnoldi factorization + const Index m_n; // dimension of A + const Index m_m; // maximum dimension of subspace V + Index m_k; // current dimension of subspace V + Matrix m_fac_V; // V matrix in the Arnoldi factorization + Matrix m_fac_H; // H matrix in the Arnoldi factorization + Vector m_fac_f; // residual in the Arnoldi factorization + RealScalar m_beta; // ||f||, B-norm of f + + // Given orthonormal basis V (w.r.t. B), find a nonzero vector f such that (V^H)Bf = 0 + // With rounding errors, we hope ||(V^H)Bf|| < eps * ||f|| // Assume that f has been properly allocated - void expand_basis(MapConstMat& V, const Index seed, Vector& f, Scalar& fnorm) + void expand_basis(MapConstMat& V, const Index seed, Vector& f, RealScalar& fnorm, Index& op_counter) { using std::sqrt; - const Scalar thresh = m_eps * sqrt(Scalar(m_n)); - Vector Vf(V.cols()); + Vector v(m_n), Vf(V.cols()); for (Index iter = 0; iter < 5; iter++) { // Randomly generate a new vector and orthogonalize it against V SimpleRandom rng(seed + 123 * iter); - f.noalias() = rng.random_vec(m_n); - // f <- f - V * V'Bf, so that f is orthogonal to V in B-norm - m_op.trans_product(V, f, Vf); + // The first try forces f to be in the range of A + if (iter == 0) + { + rng.random_vec(v); + m_op.perform_op(v.data(), f.data()); + op_counter++; + } + else + { + rng.random_vec(f); + } + // f <- f - V * (V^H)Bf, so that f is orthogonal to V in B-norm + m_op.adjoint_product(V, f, Vf); f.noalias() -= V * Vf; // fnorm <- ||f|| fnorm = m_op.norm(f); - // If fnorm is too close to zero, we try a new random vector, - // otherwise return the result - if (fnorm >= thresh) + // Compute (V^H)Bf again + m_op.adjoint_product(V, f, Vf); + // Test whether ||(V^H)Bf|| < eps * ||f|| + RealScalar ortho_err = Vf.cwiseAbs().maxCoeff(); + // If not, iteratively correct the residual + int count = 0; + while (count < 3 && ortho_err >= m_eps * fnorm) + { + // f <- f - V * Vf + f.noalias() -= V * Vf; + // beta <- ||f|| + fnorm = m_op.norm(f); + + m_op.adjoint_product(V, f, Vf); + ortho_err = Vf.cwiseAbs().maxCoeff(); + count++; + } + + // If the condition is satisfied, simply return + // Otherwise, go to the next iteration and try a new random vector + if (ortho_err < m_eps * fnorm) return; } } public: + // Copy an ArnoldiOp Arnoldi(const ArnoldiOpType& op, Index m) : - m_op(op), m_n(op.rows()), m_m(m), m_k(0), - m_near_0(TypeTraits::min() * Scalar(10)), - m_eps(Eigen::NumTraits::epsilon()) + m_op(op), m_n(op.rows()), m_m(m), m_k(0) {} - virtual ~Arnoldi() {} + // Move an ArnoldiOp + Arnoldi(ArnoldiOpType&& op, Index m) : + m_op(std::move(op)), m_n(op.rows()), m_m(m), m_k(0) + {} // Const-reference to internal structures const Matrix& matrix_V() const { return m_fac_V; } const Matrix& matrix_H() const { return m_fac_H; } const Vector& vector_f() const { return m_fac_f; } - Scalar f_norm() const { return m_beta; } + RealScalar f_norm() const { return m_beta; } Index subspace_dim() const { return m_k; } // Initialize with an operator and an initial vector void init(MapConstVec& v0, Index& op_counter) { + using std::abs; + m_fac_V.resize(m_n, m_m); m_fac_H.resize(m_m, m_m); m_fac_f.resize(m_n); m_fac_H.setZero(); // Verify the initial vector - const Scalar v0norm = m_op.norm(v0); + const RealScalar v0norm = m_op.norm(v0); if (v0norm < m_near_0) throw std::invalid_argument("initial residual vector cannot be zero"); // Points to the first column of V MapVec v(m_fac_V.data(), m_n); + // Force v to be in the range of A, i.e., v = A * v0 + m_op.perform_op(v0.data(), v.data()); + op_counter++; // Normalize - v.noalias() = v0 / v0norm; + const RealScalar vnorm = m_op.norm(v); + v /= vnorm; // Compute H and f Vector w(m_n); @@ -126,12 +161,14 @@ class Arnoldi m_fac_H(0, 0) = m_op.inner_product(v, w); m_fac_f.noalias() = w - v * m_fac_H(0, 0); - // In some cases f is zero in exact arithmetics, but due to rounding errors - // it may contain tiny fluctuations. When this happens, we force f to be zero - if (m_fac_f.cwiseAbs().maxCoeff() < m_eps) + // In some cases, H[1,1] is already an eigenvalue of A, + // so f would be zero in exact arithmetics. But due to rounding errors, + // it may contain tiny fluctuations. When this happens, we force f to be zero, + // so that it can be restarted in the subsequent Arnoldi factorization + if (m_fac_f.cwiseAbs().maxCoeff() < m_eps * abs(m_fac_H(0, 0))) { m_fac_f.setZero(); - m_beta = Scalar(0); + m_beta = RealScalar(0); } else { @@ -152,12 +189,12 @@ class Arnoldi if (from_k > m_k) { - std::stringstream msg; - msg << "Arnoldi: from_k (= " << from_k << ") is larger than the current subspace dimension (= " << m_k << ")"; - throw std::invalid_argument(msg.str()); + std::string msg = "Arnoldi: from_k (= " + std::to_string(from_k) + + ") is larger than the current subspace dimension (= " + std::to_string(m_k) + ")"; + throw std::invalid_argument(msg); } - const Scalar beta_thresh = m_eps * sqrt(Scalar(m_n)); + const RealScalar beta_thresh = m_eps * sqrt(RealScalar(m_n)); // Pre-allocate vectors Vector Vf(to_m); @@ -176,7 +213,7 @@ class Arnoldi if (m_beta < m_near_0) { MapConstMat V(m_fac_V.data(), m_n, i); // The first i columns - expand_basis(V, 2 * i, m_fac_f, m_beta); + expand_basis(V, 2 * i, m_fac_f, m_beta, op_counter); restart = true; } @@ -184,7 +221,7 @@ class Arnoldi m_fac_V.col(i).noalias() = m_fac_f / m_beta; // The (i+1)-th column // Note that H[i+1, i] equals to the unrestarted beta - m_fac_H(i, i - 1) = restart ? Scalar(0) : m_beta; + m_fac_H(i, i - 1) = restart ? Scalar(0) : Scalar(m_beta); // w <- A * v, v = m_fac_V.col(i) m_op.perform_op(&m_fac_V(0, i), w.data()); @@ -195,20 +232,20 @@ class Arnoldi MapConstMat Vs(m_fac_V.data(), m_n, i1); // h = m_fac_H(0:i, i) MapVec h(&m_fac_H(0, i), i1); - // h <- V'Bw - m_op.trans_product(Vs, w, h); + // h <- (V^H)Bw + m_op.adjoint_product(Vs, w, h); // f <- w - V * h m_fac_f.noalias() = w - Vs * h; m_beta = m_op.norm(m_fac_f); - if (m_beta > Scalar(0.717) * m_op.norm(h)) + if (m_beta > RealScalar(0.717) * m_op.norm(h)) continue; // f/||f|| is going to be the next column of V, so we need to test - // whether V'B(f/||f||) ~= 0 - m_op.trans_product(Vs, m_fac_f, Vf.head(i1)); - Scalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); + // whether (V^H)B(f/||f||) ~= 0 + m_op.adjoint_product(Vs, m_fac_f, Vf.head(i1)); + RealScalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); // If not, iteratively correct the residual int count = 0; while (count < 5 && ortho_err > m_eps * m_beta) @@ -221,7 +258,7 @@ class Arnoldi if (m_beta < beta_thresh) { m_fac_f.setZero(); - m_beta = Scalar(0); + m_beta = RealScalar(0); break; } @@ -232,7 +269,7 @@ class Arnoldi // beta <- ||f|| m_beta = m_op.norm(m_fac_f); - m_op.trans_product(Vs, m_fac_f, Vf.head(i1)); + m_op.adjoint_product(Vs, m_fac_f, Vf.head(i1)); ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); count++; } @@ -261,13 +298,21 @@ class Arnoldi // Only need to update the first k+1 columns of V // The first (m - k + i) elements of the i-th column of Q are non-zero, // and the rest are zero - void compress_V(const Matrix& Q) + // + // When V has a complex type, Q can be either real or complex + // Hense we use a generic implementation + template + void compress_V(const Eigen::MatrixBase& Q) { + using QScalar = typename Derived::Scalar; + using QVector = Eigen::Matrix; + using QMapConstVec = Eigen::Map; + Matrix Vs(m_n, m_k + 1); for (Index i = 0; i < m_k; i++) { const Index nnz = m_m - m_k + i + 1; - MapConstVec q(&Q(0, i), nnz); + QMapConstVec q(&Q(0, i), nnz); Vs.col(i).noalias() = m_fac_V.leftCols(nnz) * q; } Vs.col(m_k).noalias() = m_fac_V * Q.col(m_k); @@ -281,4 +326,4 @@ class Arnoldi } // namespace Spectra -#endif // ARNOLDI_H +#endif // SPECTRA_ARNOLDI_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h b/gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h index 2345e0fda9..386bb07856 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h +++ b/gtsam/3rdparty/Spectra/LinAlg/BKLDLT.h @@ -1,20 +1,58 @@ -// Copyright (C) 2019 Yixuan Qiu +// Copyright (C) 2019-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef BK_LDLT_H -#define BK_LDLT_H +#ifndef SPECTRA_BK_LDLT_H +#define SPECTRA_BK_LDLT_H #include #include #include +#include // std::is_same #include "../Util/CompInfo.h" namespace Spectra { +// We need a generic conj() function for both real and complex values, +// and hope that conj(x) == x if x is real-valued. However, in STL, +// conj(x) == std::complex(x, 0) for such cases, meaning that the +// return value type is not necessarily the same as x. To avoid this +// inconvenience, we define a simple class that does this task +// +// Similarly, define a real(x) function that returns x itself if +// x is real-valued, and returns std::complex(x, 0) if x is complex-valued +template +struct ScalarOp +{ + static Scalar conj(const Scalar& x) + { + return x; + } + + static Scalar real(const Scalar& x) + { + return x; + } +}; +// Specialization for complex values +template +struct ScalarOp> +{ + static std::complex conj(const std::complex& x) + { + using std::conj; + return conj(x); + } + + static std::complex real(const std::complex& x) + { + return std::complex(x.real(), RealScalar(0)); + } +}; + // Bunch-Kaufman LDLT decomposition // References: // 1. Bunch, J. R., & Kaufman, L. (1977). Some stable methods for calculating inertia and solving symmetric linear systems. @@ -27,26 +65,24 @@ template class BKLDLT { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapVec; - typedef Eigen::Map MapConstVec; - - typedef Eigen::Matrix IntVector; - typedef Eigen::Ref GenericVector; - typedef Eigen::Ref GenericMatrix; - typedef const Eigen::Ref ConstGenericMatrix; - typedef const Eigen::Ref ConstGenericVector; + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapVec = Eigen::Map; + using MapConstVec = Eigen::Map; + using IntVector = Eigen::Matrix; + using GenericVector = Eigen::Ref; + using ConstGenericVector = const Eigen::Ref; Index m_n; - Vector m_data; // storage for a lower-triangular matrix - std::vector m_colptr; // pointers to columns - IntVector m_perm; // [-2, -1, 3, 1, 4, 5]: 0 <-> 2, 1 <-> 1, 2 <-> 3, 3 <-> 1, 4 <-> 4, 5 <-> 5 - std::vector > m_permc; // compressed version of m_perm: [(0, 2), (2, 3), (3, 1)] + Vector m_data; // storage for a lower-triangular matrix + std::vector m_colptr; // pointers to columns + IntVector m_perm; // [-2, -1, 3, 1, 4, 5]: 0 <-> 2, 1 <-> 1, 2 <-> 3, 3 <-> 1, 4 <-> 4, 5 <-> 5 + std::vector> m_permc; // compressed version of m_perm: [(0, 2), (2, 3), (3, 1)] bool m_computed; - int m_info; + CompInfo m_info; // Access to elements // Pointer to the k-th column @@ -73,28 +109,38 @@ class BKLDLT } // Copy mat - shift * I to m_data - void copy_data(ConstGenericMatrix& mat, int uplo, const Scalar& shift) + template + void copy_data(const Eigen::MatrixBase& mat, int uplo, const RealScalar& shift) { - if (uplo == Eigen::Lower) + // If mat is an expression, first evaluate it into a temporary object + // This can be achieved by assigning mat to a const Eigen::Ref& + // If mat is a plain object, no temporary object is created + const Eigen::Ref& src(mat); + + // Efficient copying for column-major matrices with lower triangular part + if ((!Derived::PlainObject::IsRowMajor) && uplo == Eigen::Lower) { for (Index j = 0; j < m_n; j++) { - const Scalar* begin = &mat.coeffRef(j, j); + const Scalar* begin = &src.coeffRef(j, j); const Index len = m_n - j; std::copy(begin, begin + len, col_pointer(j)); - diag_coeff(j) -= shift; + diag_coeff(j) -= Scalar(shift); } } else { Scalar* dest = m_data.data(); - for (Index i = 0; i < m_n; i++) + for (Index j = 0; j < m_n; j++) { - for (Index j = i; j < m_n; j++, dest++) + for (Index i = j; i < m_n; i++, dest++) { - *dest = mat.coeff(i, j); + if (uplo == Eigen::Lower) + *dest = src.coeff(i, j); + else + *dest = ScalarOp::conj(src.coeff(j, i)); } - diag_coeff(i) -= shift; + diag_coeff(j) -= Scalar(shift); } } } @@ -116,12 +162,11 @@ class BKLDLT // Assume r >= k void pivoting_1x1(Index k, Index r) { + m_perm[k] = r; + // No permutation if (k == r) - { - m_perm[k] = r; return; - } // A[k, k] <-> A[r, r] std::swap(diag_coeff(k), diag_coeff(r)); @@ -130,13 +175,32 @@ class BKLDLT std::swap_ranges(&coeff(r + 1, k), col_pointer(k + 1), &coeff(r + 1, r)); // A[(k+1):(r-1), k] <-> A[r, (k+1):(r-1)] + // Note: for Hermitian matrices, also need to do conjugate Scalar* src = &coeff(k + 1, k); - for (Index j = k + 1; j < r; j++, src++) + if (std::is_same::value) { - std::swap(*src, coeff(r, j)); + // Simple swapping for real values + for (Index j = k + 1; j < r; j++, src++) + { + std::swap(*src, coeff(r, j)); + } + } + else + { + // For complex values + for (Index j = k + 1; j < r; j++, src++) + { + const Scalar src_conj = ScalarOp::conj(*src); + *src = ScalarOp::conj(coeff(r, j)); + coeff(r, j) = src_conj; + } } - m_perm[k] = r; + // A[r, k] <- Conj(A[r, k]) + if (!std::is_same::value) + { + coeff(r, k) = ScalarOp::conj(coeff(r, k)); + } } // Working on the A[k:end, k:end] submatrix @@ -173,7 +237,7 @@ class BKLDLT // Largest (in magnitude) off-diagonal element in the first column of the current reduced matrix // r is the row index // Assume k < end - Scalar find_lambda(Index k, Index& r) + RealScalar find_lambda(Index k, Index& r) { using std::abs; @@ -181,11 +245,11 @@ class BKLDLT const Scalar* end = col_pointer(k + 1); // Start with r=k+1, lambda=A[k+1, k] r = k + 1; - Scalar lambda = abs(head[1]); + RealScalar lambda = abs(head[1]); // Scan remaining elements for (const Scalar* ptr = head + 2; ptr < end; ptr++) { - const Scalar abs_elem = abs(*ptr); + const RealScalar abs_elem = abs(*ptr); if (lambda < abs_elem) { lambda = abs_elem; @@ -200,20 +264,20 @@ class BKLDLT // Largest (in magnitude) off-diagonal element in the r-th column of the current reduced matrix // p is the row index // Assume k < r < end - Scalar find_sigma(Index k, Index r, Index& p) + RealScalar find_sigma(Index k, Index r, Index& p) { using std::abs; // First search A[r+1, r], ..., A[end, r], which has the same task as find_lambda() // If r == end, we skip this search - Scalar sigma = Scalar(-1); + RealScalar sigma = RealScalar(-1); if (r < m_n - 1) sigma = find_lambda(r, p); // Then search A[k, r], ..., A[r-1, r], which maps to A[r, k], ..., A[r, r-1] for (Index j = k; j < r; j++) { - const Scalar abs_elem = abs(coeff(r, j)); + const RealScalar abs_elem = abs(coeff(r, j)); if (sigma < abs_elem) { sigma = abs_elem; @@ -226,21 +290,21 @@ class BKLDLT // Generate permutations and apply to A // Return true if the resulting pivoting is 1x1, and false if 2x2 - bool permutate_mat(Index k, const Scalar& alpha) + bool permutate_mat(Index k, const RealScalar& alpha) { using std::abs; Index r = k, p = k; - const Scalar lambda = find_lambda(k, r); + const RealScalar lambda = find_lambda(k, r); // If lambda=0, no need to interchange - if (lambda > Scalar(0)) + if (lambda > RealScalar(0)) { - const Scalar abs_akk = abs(diag_coeff(k)); + const RealScalar abs_akk = abs(diag_coeff(k)); // If |A[k, k]| >= alpha * lambda, no need to interchange if (abs_akk < alpha * lambda) { - const Scalar sigma = find_sigma(k, r, p); + const RealScalar sigma = find_sigma(k, r, p); // If sigma * |A[k, k]| >= alpha * lambda^2, no need to interchange if (sigma * abs_akk < alpha * lambda * lambda) @@ -272,7 +336,7 @@ class BKLDLT // r = k+1, so that only one permutation needs to be performed /* const Index rp_min = std::min(r, p); const Index rp_max = std::max(r, p); - if(rp_min == k + 1) + if (rp_min == k + 1) { r = rp_min; p = rp_max; } else { @@ -283,6 +347,7 @@ class BKLDLT // Permutation on A pivoting_2x2(k, r, p); + // Permutation on L interchange_rows(k, p, 0, k - 1); interchange_rows(k + 1, r, 0, k - 1); @@ -302,51 +367,138 @@ class BKLDLT { // inv(E) = [d11, d12], d11 = e22/delta, d21 = -e21/delta, d22 = e11/delta // [d21, d22] - const Scalar delta = e11 * e22 - e21 * e21; + // delta = e11 * e22 - e12 * e21 + const Scalar e12 = ScalarOp::conj(e21); + const Scalar delta = e11 * e22 - e12 * e21; std::swap(e11, e22); e11 /= delta; e22 /= delta; e21 = -e21 / delta; } - // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE - int gaussian_elimination_1x1(Index k) + // E = [e11, e12] + // [e21, e22] + // Overwrite b with x = inv(E) * b, which is equivalent to solving E * x = b + void solve_inplace_2x2( + const Scalar& e11, const Scalar& e21, const Scalar& e22, + Scalar& b1, Scalar& b2) const { - // D = 1 / A[k, k] - const Scalar akk = diag_coeff(k); - // Return NUMERICAL_ISSUE if not invertible + using std::abs; + + const Scalar e12 = ScalarOp::conj(e21); + const RealScalar e11_abs = abs(e11); + const RealScalar e21_abs = abs(e21); + // If |e11| >= |e21|, no need to exchange rows + if (e11_abs >= e21_abs) + { + const Scalar fac = e21 / e11; + const Scalar x2 = (b2 - fac * b1) / (e22 - fac * e12); + const Scalar x1 = (b1 - e12 * x2) / e11; + b1 = x1; + b2 = x2; + } + else + { + // Exchange row 1 and row 2, so the system becomes + // E* = [e21, e22], b* = [b2], x* = [x1] + // [e11, e12] [b1] [x2] + const Scalar fac = e11 / e21; + const Scalar x2 = (b1 - fac * b2) / (e12 - fac * e22); + const Scalar x1 = (b2 - e22 * x2) / e21; + b1 = x1; + b2 = x2; + } + } + + // Compute C * inv(E), which is equivalent to solving X * E = C + // X [n x 2], E [2 x 2], C [n x 2] + // X = [x1, x2], E = [e11, e12], C = [c1 c2] + // [e21, e22] + void solve_left_2x2( + const Scalar& e11, const Scalar& e21, const Scalar& e22, + const MapVec& c1, const MapVec& c2, + Eigen::Matrix& x) const + { + using std::abs; + + const Scalar e12 = ScalarOp::conj(e21); + const RealScalar e11_abs = abs(e11); + const RealScalar e12_abs = abs(e12); + // If |e11| >= |e12|, no need to exchange rows + if (e11_abs >= e12_abs) + { + const Scalar fac = e12 / e11; + // const Scalar x2 = (c2 - fac * c1) / (e22 - fac * e21); + // const Scalar x1 = (c1 - e21 * x2) / e11; + x.col(1).array() = (c2 - fac * c1).array() / (e22 - fac * e21); + x.col(0).array() = (c1 - e21 * x.col(1)).array() / e11; + } + else + { + // Exchange column 1 and column 2, so the system becomes + // X* = [x1, x2], E* = [e12, e11], C* = [c2 c1] + // [e22, e21] + const Scalar fac = e11 / e12; + // const Scalar x2 = (c1 - fac * c2) / (e21 - fac * e22); + // const Scalar x1 = (c2 - e22 * x2) / e12; + x.col(1).array() = (c1 - fac * c2).array() / (e21 - fac * e22); + x.col(0).array() = (c2 - e22 * x.col(1)).array() / e12; + } + } + + // Return value is the status, CompInfo::Successful/NumericalIssue + CompInfo gaussian_elimination_1x1(Index k) + { + // A[k, k] is known to be real-valued, so we force its imaginary + // part to be zero when Scalar is a complex type + // Interestingly, this has a significant effect on the accuracy + // and numerical stability of the final solution + const Scalar akk = ScalarOp::real(diag_coeff(k)); + diag_coeff(k) = akk; + // Return CompInfo::NumericalIssue if not invertible if (akk == Scalar(0)) - return NUMERICAL_ISSUE; + return CompInfo::NumericalIssue; - diag_coeff(k) = Scalar(1) / akk; + // [inverse] + // diag_coeff(k) = Scalar(1) / akk; - // B -= l * l' / A[k, k], B := A[(k+1):end, (k+1):end], l := L[(k+1):end, k] + // B -= l * l^H / A[k, k], B := A[(k+1):end, (k+1):end], l := L[(k+1):end, k] Scalar* lptr = col_pointer(k) + 1; const Index ldim = m_n - k - 1; MapVec l(lptr, ldim); for (Index j = 0; j < ldim; j++) { - MapVec(col_pointer(j + k + 1), ldim - j).noalias() -= (lptr[j] / akk) * l.tail(ldim - j); + Scalar l_conj = ScalarOp::conj(lptr[j]); + MapVec(col_pointer(j + k + 1), ldim - j).noalias() -= (l_conj / akk) * l.tail(ldim - j); } // l /= A[k, k] l /= akk; - return SUCCESSFUL; + return CompInfo::Successful; } - // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE - int gaussian_elimination_2x2(Index k) + // Return value is the status, CompInfo::Successful/NumericalIssue + CompInfo gaussian_elimination_2x2(Index k) { - // D = inv(E) Scalar& e11 = diag_coeff(k); Scalar& e21 = coeff(k + 1, k); Scalar& e22 = diag_coeff(k + 1); - // Return NUMERICAL_ISSUE if not invertible - if (e11 * e22 - e21 * e21 == Scalar(0)) - return NUMERICAL_ISSUE; - inverse_inplace_2x2(e11, e21, e22); + // A[k, k] and A[k+1, k+1] are known to be real-valued, + // so we force their imaginary parts to be zero when Scalar + // is a complex type + // Interestingly, this has a significant effect on the accuracy + // and numerical stability of the final solution + e11 = ScalarOp::real(e11); + e22 = ScalarOp::real(e22); + Scalar e12 = ScalarOp::conj(e21); + // Return CompInfo::NumericalIssue if not invertible + if (e11 * e22 - e12 * e21 == Scalar(0)) + return CompInfo::NumericalIssue; + + // [inverse] + // inverse_inplace_2x2(e11, e21, e22); // X = l * inv(E), l := L[(k+2):end, k:(k+1)] Scalar* l1ptr = &coeff(k + 2, k); @@ -355,35 +507,43 @@ class BKLDLT MapVec l1(l1ptr, ldim), l2(l2ptr, ldim); Eigen::Matrix X(ldim, 2); - X.col(0).noalias() = l1 * e11 + l2 * e21; - X.col(1).noalias() = l1 * e21 + l2 * e22; - - // B -= l * inv(E) * l' = X * l', B = A[(k+2):end, (k+2):end] + // [inverse] + // e12 = ScalarOp::conj(e21); + // X.col(0).noalias() = l1 * e11 + l2 * e21; + // X.col(1).noalias() = l1 * e12 + l2 * e22; + // [solve] + solve_left_2x2(e11, e21, e22, l1, l2, X); + + // B -= l * inv(E) * l^H = X * l^H, B = A[(k+2):end, (k+2):end] for (Index j = 0; j < ldim; j++) { - MapVec(col_pointer(j + k + 2), ldim - j).noalias() -= (X.col(0).tail(ldim - j) * l1ptr[j] + X.col(1).tail(ldim - j) * l2ptr[j]); + const Scalar l1j_conj = ScalarOp::conj(l1ptr[j]); + const Scalar l2j_conj = ScalarOp::conj(l2ptr[j]); + MapVec(col_pointer(j + k + 2), ldim - j).noalias() -= (X.col(0).tail(ldim - j) * l1j_conj + X.col(1).tail(ldim - j) * l2j_conj); } // l = X l1.noalias() = X.col(0); l2.noalias() = X.col(1); - return SUCCESSFUL; + return CompInfo::Successful; } public: BKLDLT() : - m_n(0), m_computed(false), m_info(NOT_COMPUTED) + m_n(0), m_computed(false), m_info(CompInfo::NotComputed) {} // Factorize mat - shift * I - BKLDLT(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) : - m_n(mat.rows()), m_computed(false), m_info(NOT_COMPUTED) + template + BKLDLT(const Eigen::MatrixBase& mat, int uplo = Eigen::Lower, const RealScalar& shift = RealScalar(0)) : + m_n(mat.rows()), m_computed(false), m_info(CompInfo::NotComputed) { compute(mat, uplo, shift); } - void compute(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) + template + void compute(const Eigen::MatrixBase& mat, int uplo = Eigen::Lower, const RealScalar& shift = RealScalar(0)) { using std::abs; @@ -399,7 +559,7 @@ class BKLDLT compute_pointer(); copy_data(mat, uplo, shift); - const Scalar alpha = (1.0 + std::sqrt(17.0)) / 8.0; + const RealScalar alpha = (1.0 + std::sqrt(17.0)) / 8.0; Index k = 0; for (k = 0; k < m_n - 1; k++) { @@ -418,17 +578,19 @@ class BKLDLT } // 3. Check status - if (m_info != SUCCESSFUL) + if (m_info != CompInfo::Successful) break; } // Invert the last 1x1 block if it exists if (k == m_n - 1) { - const Scalar akk = diag_coeff(k); + const Scalar akk = ScalarOp::real(diag_coeff(k)); + diag_coeff(k) = akk; if (akk == Scalar(0)) - m_info = NUMERICAL_ISSUE; + m_info = CompInfo::NumericalIssue; - diag_coeff(k) = Scalar(1) / diag_coeff(k); + // [inverse] + // diag_coeff(k) = Scalar(1) / diag_coeff(k); } compress_permutation(); @@ -442,7 +604,8 @@ class BKLDLT if (!m_computed) throw std::logic_error("BKLDLT: need to call compute() first"); - // PAP' = LDL' + // PAP' = LD(L^H), A = P'LD(L^H)P + // Ax = b ==> P'LD(L^H)Px = b ==> LD(L^H)Px = Pb // 1. b -> Pb Scalar* x = b.data(); MapVec res(x, m_n); @@ -452,6 +615,7 @@ class BKLDLT std::swap(x[m_permc[i].first], x[m_permc[i].second]); } + // z = D(L^H)Px // 2. Lz = Pb // If m_perm[end] < 0, then end with m_n - 3, otherwise end with m_n - 2 const Index end = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2); @@ -473,37 +637,47 @@ class BKLDLT } } + // w = (L^H)Px // 3. Dw = z for (Index i = 0; i < m_n; i++) { const Scalar e11 = diag_coeff(i); if (m_perm[i] >= 0) { - x[i] *= e11; + // [inverse] + // x[i] *= e11; + // [solve] + x[i] /= e11; } else { const Scalar e21 = coeff(i + 1, i), e22 = diag_coeff(i + 1); - const Scalar wi = x[i] * e11 + x[i + 1] * e21; - x[i + 1] = x[i] * e21 + x[i + 1] * e22; - x[i] = wi; + // [inverse] + // const Scalar e12 = ScalarOp::conj(e21); + // const Scalar wi = x[i] * e11 + x[i + 1] * e12; + // x[i + 1] = x[i] * e21 + x[i + 1] * e22; + // x[i] = wi; + // [solve] + solve_inplace_2x2(e11, e21, e22, x[i], x[i + 1]); + i++; } } - // 4. L'y = w + // y = Px + // 4. (L^H)y = w // If m_perm[end] < 0, then start with m_n - 3, otherwise start with m_n - 2 Index i = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2); for (; i >= 0; i--) { const Index ldim = m_n - i - 1; MapConstVec l(&coeff(i + 1, i), ldim); - x[i] -= res.segment(i + 1, ldim).dot(l); + x[i] -= l.dot(res.segment(i + 1, ldim)); if (m_perm[i] < 0) { MapConstVec l2(&coeff(i + 1, i - 1), ldim); - x[i - 1] -= res.segment(i + 1, ldim).dot(l2); + x[i - 1] -= l2.dot(res.segment(i + 1, ldim)); i--; } } @@ -522,9 +696,9 @@ class BKLDLT return res; } - int info() const { return m_info; } + CompInfo info() const { return m_info; } }; } // namespace Spectra -#endif // BK_LDLT_H +#endif // SPECTRA_BK_LDLT_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h b/gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h index 2845688b4f..73b787d082 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h +++ b/gtsam/3rdparty/Spectra/LinAlg/DoubleShiftQR.h @@ -1,15 +1,16 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DOUBLE_SHIFT_QR_H -#define DOUBLE_SHIFT_QR_H +#ifndef SPECTRA_DOUBLE_SHIFT_QR_H +#define SPECTRA_DOUBLE_SHIFT_QR_H #include #include // std::vector #include // std::min, std::fill, std::copy +#include // std::swap #include // std::abs, std::sqrt, std::pow #include // std::invalid_argument, std::logic_error @@ -21,31 +22,87 @@ template class DoubleShiftQR { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Matrix3X; - typedef Eigen::Matrix Vector; - typedef Eigen::Array IntArray; - - typedef Eigen::Ref GenericMatrix; - typedef const Eigen::Ref ConstGenericMatrix; - - Index m_n; // Dimension of the matrix - Matrix m_mat_H; // A copy of the matrix to be factorized - Scalar m_shift_s; // Shift constant - Scalar m_shift_t; // Shift constant - Matrix3X m_ref_u; // Householder reflectors - IntArray m_ref_nr; // How many rows does each reflector affects - // 3 - A general reflector - // 2 - A Givens rotation - // 1 - An identity transformation - const Scalar m_near_0; // a very small value, but 1.0 / m_safe_min does not overflow - // ~= 1e-307 for the "double" type - const Scalar m_eps; // the machine precision, - // e.g. ~= 1e-16 for the "double" type - const Scalar m_eps_rel; - const Scalar m_eps_abs; - bool m_computed; // Whether matrix has been factorized + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Matrix3X = Eigen::Matrix; + using Vector = Eigen::Matrix; + using IntArray = Eigen::Array; + + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; + + // A very small value, but 1.0 / m_near_0 does not overflow + // ~= 1e-307 for the "double" type + const Scalar m_near_0 = TypeTraits::min() * Scalar(10); + // The machine precision, ~= 1e-16 for the "double" type + const Scalar m_eps = TypeTraits::epsilon(); + + Index m_n; // Dimension of the matrix + Matrix m_mat_H; // A copy of the matrix to be factorized + Scalar m_shift_s; // Shift constant + Scalar m_shift_t; // Shift constant + Matrix3X m_ref_u; // Householder reflectors + IntArray m_ref_nr; // How many rows does each reflector affects + // 3 - A general reflector + // 2 - A Givens rotation + // 1 - An identity transformation + bool m_computed; // Whether matrix has been factorized + + // Compute sqrt(x1^2 + x2^2 + x3^2) wit high precision + static Scalar stable_norm3(Scalar x1, Scalar x2, Scalar x3) + { + using std::abs; + using std::sqrt; + + x1 = abs(x1); + x2 = abs(x2); + x3 = abs(x3); + // Make x1 >= {x2, x3} + if (x1 < x2) + std::swap(x1, x2); + if (x1 < x3) + std::swap(x1, x3); + // If x1 is too small, return 0 + const Scalar near_0 = TypeTraits::min() * Scalar(10); + if (x1 < near_0) + return Scalar(0); + + const Scalar r2 = x2 / x1, r3 = x3 / x1; + // We choose a cutoff such that cutoff^4 < eps + // If max(r2, r3) > cutoff, use the standard way; otherwise use Taylor series expansion + // to avoid an explicit sqrt() call that may lose precision + const Scalar eps = TypeTraits::epsilon(); + const Scalar cutoff = Scalar(0.1) * pow(eps, Scalar(0.25)); + Scalar r = r2 * r2 + r3 * r3; + r = (r2 >= cutoff || r3 >= cutoff) ? + sqrt(Scalar(1) + r) : + (Scalar(1) + r * (Scalar(0.5) - Scalar(0.125) * r)); // sqrt(1 + t) ~= 1 + t/2 - t^2/8 + return x1 * r; + } + + // x[i] <- x[i] / r, r = sqrt(x1^2 + x2^2 + x3^2) + // Assume |x1| >= {|x2|, |x3|}, x1 != 0 + static void stable_scaling(Scalar& x1, Scalar& x2, Scalar& x3) + { + using std::abs; + using std::pow; + using std::sqrt; + + const Scalar x1sign = (x1 > Scalar(0)) ? Scalar(1) : Scalar(-1); + x1 = abs(x1); + // Use the same method as in stable_norm3() + const Scalar r2 = x2 / x1, r3 = x3 / x1; + const Scalar eps = TypeTraits::epsilon(); + const Scalar cutoff = Scalar(0.1) * pow(eps, Scalar(0.25)); + Scalar r = r2 * r2 + r3 * r3; + // r = 1/sqrt(1 + r2^2 + r3^2) + r = (abs(r2) >= cutoff || abs(r3) >= cutoff) ? + Scalar(1) / sqrt(Scalar(1) + r) : + (Scalar(1) - r * (Scalar(0.5) - Scalar(0.375) * r)); // 1/sqrt(1 + t) ~= 1 - t * (1/2 - (3/8) * t) + x1 = x1sign * r; + x2 = r2 * r; + x3 = r3 * r; + } void compute_reflector(const Scalar& x1, const Scalar& x2, const Scalar& x3, Index ind) { @@ -53,42 +110,39 @@ class DoubleShiftQR Scalar* u = &m_ref_u.coeffRef(0, ind); unsigned char* nr = m_ref_nr.data(); + const Scalar x2m = abs(x2), x3m = abs(x3); + // If both x2 and x3 are zero, nr is 1, and we early exit + if (x2m < m_near_0 && x3m < m_near_0) + { + nr[ind] = 1; + return; + } + // In general case the reflector affects 3 rows - nr[ind] = 3; - Scalar x2x3 = Scalar(0); // If x3 is zero, decrease nr by 1 - if (abs(x3) < m_near_0) + nr[ind] = (x3m < m_near_0) ? 2 : 3; + const Scalar x_norm = (x3m < m_near_0) ? Eigen::numext::hypot(x1, x2) : stable_norm3(x1, x2, x3); + + // x1' = x1 - rho * ||x|| + // rho = -sign(x1), if x1 == 0, we choose rho = 1 + const Scalar rho = (x1 <= Scalar(0)) - (x1 > Scalar(0)); + const Scalar x1_new = x1 - rho * x_norm, x1m = abs(x1_new); + // Copy x to u + u[0] = x1_new; + u[1] = x2; + u[2] = x3; + if (x1m >= x2m && x1m >= x3m) { - // If x2 is also zero, nr will be 1, and we can exit this function - if (abs(x2) < m_near_0) - { - nr[ind] = 1; - return; - } - else - { - nr[ind] = 2; - } - x2x3 = abs(x2); + stable_scaling(u[0], u[1], u[2]); } - else + else if (x2m >= x1m && x2m >= x3m) { - x2x3 = Eigen::numext::hypot(x2, x3); + stable_scaling(u[1], u[0], u[2]); } - - // x1' = x1 - rho * ||x|| - // rho = -sign(x1), if x1 == 0, we choose rho = 1 - Scalar x1_new = x1 - ((x1 <= 0) - (x1 > 0)) * Eigen::numext::hypot(x1, x2x3); - Scalar x_norm = Eigen::numext::hypot(x1_new, x2x3); - // Double check the norm of new x - if (x_norm < m_near_0) + else { - nr[ind] = 1; - return; + stable_scaling(u[2], u[0], u[1]); } - u[0] = x1_new / x_norm; - u[1] = x2 / x_norm; - u[2] = x3 / x_norm; } void compute_reflector(const Scalar* x, Index ind) @@ -138,7 +192,7 @@ class DoubleShiftQR // Apply the first reflector apply_PX(m_mat_H.block(il, il, 3, m_n - il), m_n, il); - apply_XP(m_mat_H.block(0, il, il + std::min(bsize, Index(4)), 3), m_n, il); + apply_XP(m_mat_H.block(0, il, il + (std::min)(bsize, Index(4)), 3), m_n, il); // Calculate the following reflectors // If entering this loop, block size is at least 4. @@ -147,7 +201,7 @@ class DoubleShiftQR compute_reflector(&m_mat_H.coeffRef(il + i, il + i - 1), il + i); // Apply the reflector to X apply_PX(m_mat_H.block(il + i, il + i - 1, 3, m_n - il - i + 1), m_n, il + i); - apply_XP(m_mat_H.block(0, il + i, il + std::min(bsize, Index(i + 4)), 3), m_n, il + i); + apply_XP(m_mat_H.block(0, il + i, il + (std::min)(bsize, Index(i + 4)), 3), m_n, il + i); } // The last reflector @@ -168,10 +222,8 @@ class DoubleShiftQR if (nr == 1) return; - const Scalar u0 = m_ref_u.coeff(0, u_ind), - u1 = m_ref_u.coeff(1, u_ind); - const Scalar u0_2 = Scalar(2) * u0, - u1_2 = Scalar(2) * u1; + const Scalar u0 = m_ref_u.coeff(0, u_ind), u1 = m_ref_u.coeff(1, u_ind); + const Scalar u0_2 = Scalar(2) * u0, u1_2 = Scalar(2) * u1; const Index nrow = X.rows(); const Index ncol = X.cols(); @@ -228,10 +280,8 @@ class DoubleShiftQR if (nr == 1) return; - const Scalar u0 = m_ref_u.coeff(0, u_ind), - u1 = m_ref_u.coeff(1, u_ind); - const Scalar u0_2 = Scalar(2) * u0, - u1_2 = Scalar(2) * u1; + const Scalar u0 = m_ref_u.coeff(0, u_ind), u1 = m_ref_u.coeff(1, u_ind); + const Scalar u0_2 = Scalar(2) * u0, u1_2 = Scalar(2) * u1; const int nrow = X.rows(); const int ncol = X.cols(); @@ -267,10 +317,6 @@ class DoubleShiftQR public: DoubleShiftQR(Index size) : m_n(size), - m_near_0(TypeTraits::min() * Scalar(10)), - m_eps(Eigen::NumTraits::epsilon()), - m_eps_rel(m_eps), - m_eps_abs(m_near_0 * (m_n / m_eps)), m_computed(false) {} @@ -281,10 +327,6 @@ class DoubleShiftQR m_shift_t(t), m_ref_u(3, m_n), m_ref_nr(m_n), - m_near_0(TypeTraits::min() * Scalar(10)), - m_eps(Eigen::NumTraits::epsilon()), - m_eps_rel(m_eps), - m_eps_abs(m_near_0 * (m_n / m_eps)), m_computed(false) { compute(mat, s, t); @@ -305,19 +347,25 @@ class DoubleShiftQR m_ref_nr.resize(m_n); // Make a copy of mat - std::copy(mat.data(), mat.data() + mat.size(), m_mat_H.data()); + m_mat_H.noalias() = mat; // Obtain the indices of zero elements in the subdiagonal, // so that H can be divided into several blocks + const Scalar eps_abs = m_near_0 * (m_n / m_eps); + const Scalar eps_rel = m_eps; std::vector zero_ind; zero_ind.reserve(m_n - 1); zero_ind.push_back(0); Scalar* Hii = m_mat_H.data(); - for (Index i = 0; i < m_n - 2; i++, Hii += (m_n + 1)) + for (Index i = 0; i < m_n - 1; i++, Hii += (m_n + 1)) { + // Hii[0] => m_mat_H(i, i) // Hii[1] => m_mat_H(i + 1, i) + // Hii[m_n + 1] => m_mat_H(i + 1, i + 1) const Scalar h = abs(Hii[1]); - if (h <= 0 || h <= m_eps_rel * (abs(Hii[0]) + abs(Hii[m_n + 1]))) + // Deflate small sub-diagonal elements + const Scalar diag = abs(Hii[0]) + abs(Hii[m_n + 1]); + if (h <= eps_abs || h <= eps_rel * diag) { Hii[1] = 0; zero_ind.push_back(i + 1); @@ -328,7 +376,8 @@ class DoubleShiftQR } zero_ind.push_back(m_n); - for (std::vector::size_type i = 0; i < zero_ind.size() - 1; i++) + const Index len = zero_ind.size() - 1; + for (Index i = 0; i < len; i++) { const Index start = zero_ind[i]; const Index end = zero_ind[i + 1] - 1; @@ -336,6 +385,16 @@ class DoubleShiftQR update_block(start, end); } + // Deflation on the computed result + Hii = m_mat_H.data(); + for (Index i = 0; i < m_n - 1; i++, Hii += (m_n + 1)) + { + const Scalar h = abs(Hii[1]); + const Scalar diag = abs(Hii[0]) + abs(Hii[m_n + 1]); + if (h <= eps_abs || h <= eps_rel * diag) + Hii[1] = 0; + } + m_computed = true; } @@ -381,4 +440,4 @@ class DoubleShiftQR } // namespace Spectra -#endif // DOUBLE_SHIFT_QR_H +#endif // SPECTRA_DOUBLE_SHIFT_QR_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/Lanczos.h b/gtsam/3rdparty/Spectra/LinAlg/Lanczos.h index 53cf52be85..da1f089602 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/Lanczos.h +++ b/gtsam/3rdparty/Spectra/LinAlg/Lanczos.h @@ -1,16 +1,16 @@ -// Copyright (C) 2018-2019 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef LANCZOS_H -#define LANCZOS_H +#ifndef SPECTRA_LANCZOS_H +#define SPECTRA_LANCZOS_H #include #include // std::sqrt +#include // std::forward #include // std::invalid_argument -#include // std::stringstream #include "Arnoldi.h" @@ -27,13 +27,15 @@ template class Lanczos : public Arnoldi { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapMat; - typedef Eigen::Map MapVec; - typedef Eigen::Map MapConstMat; - typedef Eigen::Map MapConstVec; + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapMat = Eigen::Map; + using MapVec = Eigen::Map; + using MapConstMat = Eigen::Map; + using RealMatrix = Eigen::Matrix; using Arnoldi::m_op; using Arnoldi::m_n; @@ -47,13 +49,16 @@ class Lanczos : public Arnoldi using Arnoldi::m_eps; public: - Lanczos(const ArnoldiOpType& op, Index m) : - Arnoldi(op, m) + // Forward parameter `op` to the constructor of Arnoldi + template + Lanczos(T&& op, Index m) : + Arnoldi(std::forward(op), m) {} // Lanczos factorization starting from step-k - void factorize_from(Index from_k, Index to_m, Index& op_counter) + void factorize_from(Index from_k, Index to_m, Index& op_counter) override { + using std::abs; using std::sqrt; if (to_m <= from_k) @@ -61,12 +66,13 @@ class Lanczos : public Arnoldi if (from_k > m_k) { - std::stringstream msg; - msg << "Lanczos: from_k (= " << from_k << ") is larger than the current subspace dimension (= " << m_k << ")"; - throw std::invalid_argument(msg.str()); + std::string msg = "Lanczos: from_k (= " + std::to_string(from_k) + + ") is larger than the current subspace dimension (= " + std::to_string(m_k) + ")"; + throw std::invalid_argument(msg); } - const Scalar beta_thresh = m_eps * sqrt(Scalar(m_n)); + const RealScalar beta_thresh = m_eps * sqrt(RealScalar(m_n)); + const RealScalar eps_sqrt = sqrt(m_eps); // Pre-allocate vectors Vector Vf(to_m); @@ -78,47 +84,70 @@ class Lanczos : public Arnoldi for (Index i = from_k; i <= to_m - 1; i++) { - bool restart = false; // If beta = 0, then the next V is not full rank // We need to generate a new residual vector that is orthogonal // to the current V, which we call a restart - if (m_beta < m_near_0) + // + // A simple criterion is beta < near_0, but it may be too stringent + // Another heuristic is to test whether (V^H)B(f/||f||) ~= 0 when ||f|| is small, + // and to reduce the computational cost, we only use the latest Vi + + // Test the first criterion + bool restart = (m_beta < m_near_0); + // If not met, test the second criterion + // v is the (i+1)-th column of V + MapVec v(&m_fac_V(0, i), m_n); + if (!restart) + { + // Save v <- f / ||f|| to the (i+1)-th column of V + v.noalias() = m_fac_f / m_beta; + if (m_beta < eps_sqrt) + { + // Test (Vi^H)v + const Scalar Viv = m_op.inner_product(m_fac_V.col(i - 1), v); + // Restart V if (Vi^H)v is much larger than eps + restart = (abs(Viv) > eps_sqrt); + } + } + + if (restart) { MapConstMat V(m_fac_V.data(), m_n, i); // The first i columns - this->expand_basis(V, 2 * i, m_fac_f, m_beta); - restart = true; + this->expand_basis(V, 2 * i, m_fac_f, m_beta, op_counter); + v.noalias() = m_fac_f / m_beta; } - // v <- f / ||f|| - MapVec v(&m_fac_V(0, i), m_n); // The (i+1)-th column - v.noalias() = m_fac_f / m_beta; + // Whether there is a restart or not, right now the (i+1)-th column of V + // contains f / ||f|| // Note that H[i+1, i] equals to the unrestarted beta - m_fac_H(i, i - 1) = restart ? Scalar(0) : m_beta; + m_fac_H(i, i - 1) = restart ? Scalar(0) : Scalar(m_beta); + m_fac_H(i - 1, i) = m_fac_H(i, i - 1); // Due to symmetry // w <- A * v m_op.perform_op(v.data(), w.data()); op_counter++; - // H[i+1, i+1] = = v'Bw - m_fac_H(i - 1, i) = m_fac_H(i, i - 1); // Due to symmetry - m_fac_H(i, i) = m_op.inner_product(v, w); - - // f <- w - V * V'Bw = w - H[i+1, i] * V{i} - H[i+1, i+1] * V{i+1} + // f <- w - V * (V^H)Bw = w - H[i+1, i] * V{i} - H[i+1, i+1] * V{i+1} // If restarting, we know that H[i+1, i] = 0 - if (restart) - m_fac_f.noalias() = w - m_fac_H(i, i) * v; - else - m_fac_f.noalias() = w - m_fac_H(i, i - 1) * m_fac_V.col(i - 1) - m_fac_H(i, i) * v; + // First do w <- w - H[i+1, i] * V{i}, see the discussions in Section 2.3 of + // Cullum and Willoughby (2002). Lanczos Algorithms for Large Symmetric Eigenvalue Computations: Vol. 1 + if (!restart) + w.noalias() -= m_fac_H(i, i - 1) * m_fac_V.col(i - 1); + + // H[i+1, i+1] = = (v^H)Bw + m_fac_H(i, i) = m_op.inner_product(v, w); + // f <- w - H[i+1, i+1] * V{i+1} + m_fac_f.noalias() = w - m_fac_H(i, i) * v; m_beta = m_op.norm(m_fac_f); // f/||f|| is going to be the next column of V, so we need to test - // whether V'B(f/||f||) ~= 0 + // whether (V^H)B(f/||f||) ~= 0 const Index i1 = i + 1; MapMat Vs(m_fac_V.data(), m_n, i1); // The first (i+1) columns - m_op.trans_product(Vs, m_fac_f, Vf.head(i1)); - Scalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); + m_op.adjoint_product(Vs, m_fac_f, Vf.head(i1)); + RealScalar ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); // If not, iteratively correct the residual int count = 0; while (count < 5 && ortho_err > m_eps * m_beta) @@ -131,7 +160,7 @@ class Lanczos : public Arnoldi if (m_beta < beta_thresh) { m_fac_f.setZero(); - m_beta = Scalar(0); + m_beta = RealScalar(0); break; } @@ -144,7 +173,7 @@ class Lanczos : public Arnoldi // beta <- ||f|| m_beta = m_op.norm(m_fac_f); - m_op.trans_product(Vs, m_fac_f, Vf.head(i1)); + m_op.adjoint_product(Vs, m_fac_f, Vf.head(i1)); ortho_err = Vf.head(i1).cwiseAbs().maxCoeff(); count++; } @@ -155,13 +184,36 @@ class Lanczos : public Arnoldi } // Apply H -> Q'HQ, where Q is from a tridiagonal QR decomposition - void compress_H(const TridiagQR& decomp) + // Function overloading here, not overriding + // + // Note that H is by nature a real symmetric matrix, but it may be stored + // as a complex matrix (e.g. in HermEigsSolver). + // Therefore, if m_fac_H has a real type (as in SymEigsSolver), then we + // directly overwrite m_fac_H. Otherwise, m_fac_H has a complex type + // (as in HermEigsSolver), so we first compute the real-typed result, + // and then cast to the complex type. This is done in the TridiagQR class + void compress_H(const TridiagQR& decomp) { decomp.matrix_QtHQ(m_fac_H); m_k--; } + + // In some cases we know that H has the form H = [X e 0], + // [e' s 0] + // [0 0 D] + // where X is an irreducible tridiagonal matrix, D is a diagonal matrix, + // s is a scalar, and e = (0, ..., 0, eps), eps ~= 0 + // + // In this case we can force H[m+1, m] = H[m, m+1] = 0 and H[m+1, m+1] = s, + // where m is the size of X + void deflate_H(Index irr_size, const Scalar& s) + { + m_fac_H(irr_size, irr_size - 1) = Scalar(0); + m_fac_H(irr_size - 1, irr_size) = Scalar(0); + m_fac_H(irr_size, irr_size) = s; + } }; } // namespace Spectra -#endif // LANCZOS_H +#endif // SPECTRA_LANCZOS_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/Orthogonalization.h b/gtsam/3rdparty/Spectra/LinAlg/Orthogonalization.h new file mode 100644 index 0000000000..22c9f45ca3 --- /dev/null +++ b/gtsam/3rdparty/Spectra/LinAlg/Orthogonalization.h @@ -0,0 +1,141 @@ +// Copyright (C) 2020 Netherlands eScience Center +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_ORTHOGONALIZATION_H +#define SPECTRA_ORTHOGONALIZATION_H + +#include +#include + +namespace Spectra { + +/// Check if the number of columns to skip is +/// larger than 0 but smaller than the total number +/// of columns of the matrix +/// \param in_output Matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void assert_left_cols_to_skip(Matrix& in_output, Eigen::Index left_cols_to_skip) +{ + assert(in_output.cols() > left_cols_to_skip && "left_cols_to_skip is larger than columns of matrix"); + assert(left_cols_to_skip >= 0 && "left_cols_to_skip is negative"); +} + +/// If the the number of columns to skip is null, +/// normalize the first column and set left_cols_to_skip=1 +/// \param in_output Matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +/// \return Actual number of left columns to skip +template +Eigen::Index treat_first_col(Matrix& in_output, Eigen::Index left_cols_to_skip) +{ + if (left_cols_to_skip == 0) + { + in_output.col(0).normalize(); + left_cols_to_skip = 1; + } + return left_cols_to_skip; +} + +/// Orthogonalize the in_output matrix using a QR decomposition +/// \param in_output Matrix to be orthogonalized +template +void QR_orthogonalisation(Matrix& in_output) +{ + using InternalMatrix = Eigen::Matrix; + Eigen::Index nrows = in_output.rows(); + Eigen::Index ncols = in_output.cols(); + ncols = (std::min)(nrows, ncols); + InternalMatrix I = InternalMatrix::Identity(nrows, ncols); + Eigen::HouseholderQR qr(in_output); + in_output.leftCols(ncols).noalias() = qr.householderQ() * I; +} + +/// Orthogonalize the in_output matrix using a modified Gram Schmidt process +/// \param in_output matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void MGS_orthogonalisation(Matrix& in_output, Eigen::Index left_cols_to_skip = 0) +{ + assert_left_cols_to_skip(in_output, left_cols_to_skip); + left_cols_to_skip = treat_first_col(in_output, left_cols_to_skip); + + for (Eigen::Index k = left_cols_to_skip; k < in_output.cols(); ++k) + { + for (Eigen::Index j = 0; j < k; j++) + { + in_output.col(k) -= in_output.col(j).dot(in_output.col(k)) * in_output.col(j); + } + in_output.col(k).normalize(); + } +} + +/// Orthogonalize the in_output matrix using a Gram Schmidt process +/// \param in_output matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void GS_orthogonalisation(Matrix& in_output, Eigen::Index left_cols_to_skip = 0) +{ + assert_left_cols_to_skip(in_output, left_cols_to_skip); + left_cols_to_skip = treat_first_col(in_output, left_cols_to_skip); + + for (Eigen::Index j = left_cols_to_skip; j < in_output.cols(); ++j) + { + in_output.col(j) -= in_output.leftCols(j) * (in_output.leftCols(j).transpose() * in_output.col(j)); + in_output.col(j).normalize(); + } +} + +/// Orthogonalize the subspace spanned by right columns of in_output +/// against the subspace spanned by left columns +/// It assumes that the left columns are already orthogonal and normalized, +/// and it does not orthogonalize the left columns against each other +/// \param in_output Matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void subspace_orthogonalisation(Matrix& in_output, Eigen::Index left_cols_to_skip) +{ + assert_left_cols_to_skip(in_output, left_cols_to_skip); + if (left_cols_to_skip == 0) + { + return; + } + + Eigen::Index right_cols_to_ortho = in_output.cols() - left_cols_to_skip; + in_output.rightCols(right_cols_to_ortho) -= in_output.leftCols(left_cols_to_skip) * + (in_output.leftCols(left_cols_to_skip).transpose() * in_output.rightCols(right_cols_to_ortho)); +} + +/// Orthogonalize the in_output matrix using a Jens process +/// The subspace spanned by right columns are first orthogonalized +/// agains the left columns, and then a QR decomposition is applied on the right columns +/// to make them orthogonalized agains each other +/// \param in_output Matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void JensWehner_orthogonalisation(Matrix& in_output, Eigen::Index left_cols_to_skip = 0) +{ + assert_left_cols_to_skip(in_output, left_cols_to_skip); + + Eigen::Index right_cols_to_ortho = in_output.cols() - left_cols_to_skip; + subspace_orthogonalisation(in_output, left_cols_to_skip); + Eigen::Ref right_cols = in_output.rightCols(right_cols_to_ortho); + QR_orthogonalisation(right_cols); +} + +/// Orthogonalize the in_output matrix using a twice-is-enough Jens process +/// \param in_output Matrix to be orthogonalized +/// \param left_cols_to_skip Number of left columns to be left untouched +template +void twice_is_enough_orthogonalisation(Matrix& in_output, Eigen::Index left_cols_to_skip = 0) +{ + JensWehner_orthogonalisation(in_output, left_cols_to_skip); + JensWehner_orthogonalisation(in_output, left_cols_to_skip); +} + +} // namespace Spectra + +#endif // SPECTRA_ORTHOGONALIZATION_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/RitzPairs.h b/gtsam/3rdparty/Spectra/LinAlg/RitzPairs.h new file mode 100644 index 0000000000..09bda61276 --- /dev/null +++ b/gtsam/3rdparty/Spectra/LinAlg/RitzPairs.h @@ -0,0 +1,130 @@ +// Copyright (C) 2020 Netherlands eScience Center +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_RITZ_PAIRS_H +#define SPECTRA_RITZ_PAIRS_H + +#include +#include + +#include "../Util/SelectionRule.h" + +namespace Spectra { + +template +class SearchSpace; + +/// This class handles the creation and manipulation of Ritz eigen pairs +/// for iterative eigensolvers such as Davidson, Jacobi-Davidson, etc. +template +class RitzPairs +{ +private: + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using Array = Eigen::Array; + using BoolArray = Eigen::Array; + + Vector m_values; // eigenvalues + Matrix m_small_vectors; // eigenvectors of the small problem, makes restart cheaper. + Matrix m_vectors; // Ritz (or harmonic Ritz) eigenvectors + Matrix m_residues; // residues of the pairs + BoolArray m_root_converged; + +public: + RitzPairs() = default; + + /// Compute the eigen values/vectors + /// + /// \param search_space Instance of the class handling the search space + /// \return Eigen::ComputationalInfo Whether small eigenvalue problem worked + Eigen::ComputationInfo compute_eigen_pairs(const SearchSpace& search_space); + + /// Returns the size of the ritz eigen pairs + /// + /// \return Eigen::Index Number of pairs + Index size() const { return m_values.size(); } + + /// Sort the eigen pairs according to the selection rule + /// + /// \param selection Sorting rule + void sort(SortRule selection) + { + std::vector ind = argsort(selection, m_values); + RitzPairs temp = *this; + for (Index i = 0; i < size(); i++) + { + m_values[i] = temp.m_values[ind[i]]; + m_vectors.col(i) = temp.m_vectors.col(ind[i]); + m_residues.col(i) = temp.m_residues.col(ind[i]); + m_small_vectors.col(i) = temp.m_small_vectors.col(ind[i]); + } + } + + /// Checks if the algorithm has converged and updates root_converged + /// + /// \param tol Tolerance for convergence + /// \param number_eigenvalue Number of request eigenvalues + /// \return bool true if all eigenvalues are converged + bool check_convergence(Scalar tol, Index number_eigenvalues) + { + const Array norms = m_residues.colwise().norm(); + bool converged = true; + m_root_converged = BoolArray::Zero(norms.size()); + for (Index j = 0; j < norms.size(); j++) + { + m_root_converged[j] = (norms[j] < tol); + if (j < number_eigenvalues) + { + converged &= (norms[j] < tol); + } + } + return converged; + } + + const Matrix& ritz_vectors() const { return m_vectors; } + const Vector& ritz_values() const { return m_values; } + const Matrix& small_ritz_vectors() const { return m_small_vectors; } + const Matrix& residues() const { return m_residues; } + const BoolArray& converged_eigenvalues() const { return m_root_converged; } +}; + +} // namespace Spectra + +#include "SearchSpace.h" + +namespace Spectra { + +/// Creates the small space matrix and computes its eigen pairs +/// Also computes the ritz vectors and residues +/// +/// \param search_space Instance of the SearchSpace class +template +Eigen::ComputationInfo RitzPairs::compute_eigen_pairs(const SearchSpace& search_space) +{ + const Matrix& basis_vectors = search_space.basis_vectors(); + const Matrix& op_basis_prod = search_space.operator_basis_product(); + + // Form the small eigenvalue + Matrix small_matrix = basis_vectors.transpose() * op_basis_prod; + + // Small eigenvalue problem + Eigen::SelfAdjointEigenSolver eigen_solver(small_matrix); + m_values = eigen_solver.eigenvalues(); + m_small_vectors = eigen_solver.eigenvectors(); + + // Ritz vectors + m_vectors = basis_vectors * m_small_vectors; + + // Residues + m_residues = op_basis_prod * m_small_vectors - m_vectors * m_values.asDiagonal(); + return eigen_solver.info(); +} + +} // namespace Spectra + +#endif // SPECTRA_RITZ_PAIRS_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/SearchSpace.h b/gtsam/3rdparty/Spectra/LinAlg/SearchSpace.h new file mode 100644 index 0000000000..2122c9f446 --- /dev/null +++ b/gtsam/3rdparty/Spectra/LinAlg/SearchSpace.h @@ -0,0 +1,96 @@ +// Copyright (C) 2020 Netherlands eScience Center +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SEARCH_SPACE_H +#define SPECTRA_SEARCH_SPACE_H + +#include + +#include "RitzPairs.h" +#include "Orthogonalization.h" + +namespace Spectra { + +/// This class handles the creation and manipulation of the search space +/// for iterative eigensolvers such as Davidson, Jacobi-Davidson, etc. +template +class SearchSpace +{ +private: + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + + Matrix m_basis_vectors; + Matrix m_op_basis_product; + + /// Append new vector to the basis + /// + /// \param new_vect Matrix of new correction vectors + void append_new_vectors_to_basis(const Matrix& new_vect) + { + Index num_update = new_vect.cols(); + m_basis_vectors.conservativeResize(Eigen::NoChange, m_basis_vectors.cols() + num_update); + m_basis_vectors.rightCols(num_update).noalias() = new_vect; + } + +public: + SearchSpace() = default; + + /// Returns the current size of the search space + Index size() const { return m_basis_vectors.cols(); } + + void initialize_search_space(const Eigen::Ref& initial_vectors) + { + m_basis_vectors = initial_vectors; + m_op_basis_product = Matrix(initial_vectors.rows(), 0); + } + + /// Updates the matrix formed by the operator applied to the search space + /// after the addition of new vectors in the search space. Only the product + /// of the operator with the new vectors is computed and the result is appended + /// to the op_basis_product member variable + /// + /// \param OpType Operator representing the matrix + template + void update_operator_basis_product(OpType& op) + { + Index nvec = m_basis_vectors.cols() - m_op_basis_product.cols(); + m_op_basis_product.conservativeResize(Eigen::NoChange, m_basis_vectors.cols()); + m_op_basis_product.rightCols(nvec).noalias() = op * m_basis_vectors.rightCols(nvec); + } + + /// Restart the search space by reducing the basis vector to the last + /// Ritz eigenvector + /// + /// \param ritz_pair Instance of a RitzPair class + /// \param size Size of the restart + void restart(const RitzPairs& ritz_pairs, Index size) + { + m_basis_vectors = ritz_pairs.ritz_vectors().leftCols(size); + m_op_basis_product = m_op_basis_product * ritz_pairs.small_ritz_vectors().leftCols(size); + } + + /// Append new vectors to the search space and + /// orthogonalize the resulting matrix + /// + /// \param new_vect Matrix of new correction vectors + void extend_basis(const Matrix& new_vect) + { + Index left_cols_to_skip = size(); + append_new_vectors_to_basis(new_vect); + twice_is_enough_orthogonalisation(m_basis_vectors, left_cols_to_skip); + } + + /// Returns the basis vectors + const Matrix& basis_vectors() const { return m_basis_vectors; } + + /// Returns the operator applied to basis vector + const Matrix& operator_basis_product() const { return m_op_basis_product; } +}; + +} // namespace Spectra + +#endif // SPECTRA_SEARCH_SPACE_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h b/gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h index 122e0e5518..cc2fb4ac19 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h +++ b/gtsam/3rdparty/Spectra/LinAlg/TridiagEigen.h @@ -2,14 +2,14 @@ // // Copyright (C) 2008-2010 Gael Guennebaud // Copyright (C) 2010 Jitse Niesen -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef TRIDIAG_EIGEN_H -#define TRIDIAG_EIGEN_H +#ifndef SPECTRA_TRIDIAG_EIGEN_H +#define SPECTRA_TRIDIAG_EIGEN_H #include #include @@ -23,25 +23,22 @@ template class TridiagEigen { private: - typedef Eigen::Index Index; + using Index = Eigen::Index; // For convenience in adapting the tridiagonal_qr_step() function - typedef Scalar RealScalar; - - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - - typedef Eigen::Ref GenericMatrix; - typedef const Eigen::Ref ConstGenericMatrix; + using RealScalar = Scalar; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; Index m_n; Vector m_main_diag; // Main diagonal elements of the matrix Vector m_sub_diag; // Sub-diagonal elements of the matrix Matrix m_evecs; // To store eigenvectors - bool m_computed; - const Scalar m_near_0; // a very small value, ~= 1e-307 for the "double" type // Adapted from Eigen/src/Eigenvaleus/SelfAdjointEigenSolver.h + // Francis implicit QR step. static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, @@ -49,6 +46,7 @@ class TridiagEigen { using std::abs; + // Wilkinson Shift. RealScalar td = (diag[end - 1] - diag[end]) * RealScalar(0.5); RealScalar e = subdiag[end - 1]; // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still @@ -57,14 +55,14 @@ class TridiagEigen // RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2)); // This explain the following, somewhat more complicated, version: RealScalar mu = diag[end]; - if (td == Scalar(0)) + if (td == RealScalar(0)) mu -= abs(e); - else + else if (e != RealScalar(0)) { - RealScalar e2 = Eigen::numext::abs2(subdiag[end - 1]); - RealScalar h = Eigen::numext::hypot(td, e); + const RealScalar e2 = Eigen::numext::abs2(e); + const RealScalar h = Eigen::numext::hypot(td, e); if (e2 == RealScalar(0)) - mu -= (e / (td + (td > RealScalar(0) ? RealScalar(1) : RealScalar(-1)))) * (e / h); + mu -= e / ((td + (td > RealScalar(0) ? h : -h)) / e); else mu -= e2 / (td + (td > RealScalar(0) ? h : -h)); } @@ -72,7 +70,9 @@ class TridiagEigen RealScalar x = diag[start] - mu; RealScalar z = subdiag[start]; Eigen::Map q(matrixQ, n, n); - for (Index k = start; k < end; ++k) + // If z ever becomes zero, the Givens rotation will be the identity and + // z will stay zero for all future iterations. + for (Index k = start; k < end && z != RealScalar(0); ++k) { Eigen::JacobiRotation rot; rot.makeGivens(x, z); @@ -91,8 +91,8 @@ class TridiagEigen if (k > start) subdiag[k - 1] = c * subdiag[k - 1] - s * z; + // "Chasing the bulge" to return to triangular form. x = subdiag[k]; - if (k < end - 1) { z = -s * subdiag[k + 1]; @@ -107,13 +107,11 @@ class TridiagEigen public: TridiagEigen() : - m_n(0), m_computed(false), - m_near_0(TypeTraits::min() * Scalar(10)) + m_n(0), m_computed(false) {} TridiagEigen(ConstGenericMatrix& mat) : - m_n(mat.rows()), m_computed(false), - m_near_0(TypeTraits::min() * Scalar(10)) + m_n(mat.rows()), m_computed(false) { compute(mat); } @@ -122,6 +120,10 @@ class TridiagEigen { using std::abs; + // A very small value, but 1.0 / near_0 does not overflow + // ~= 1e-307 for the "double" type + const Scalar near_0 = TypeTraits::min() * Scalar(10); + m_n = mat.rows(); if (m_n != mat.cols()) throw std::invalid_argument("TridiagEigen: matrix must be square"); @@ -132,10 +134,10 @@ class TridiagEigen m_evecs.setIdentity(); // Scale matrix to improve stability - const Scalar scale = std::max(mat.diagonal().cwiseAbs().maxCoeff(), - mat.diagonal(-1).cwiseAbs().maxCoeff()); + const Scalar scale = (std::max)(mat.diagonal().cwiseAbs().maxCoeff(), + mat.diagonal(-1).cwiseAbs().maxCoeff()); // If scale=0, mat is a zero matrix, so we can early stop - if (scale < m_near_0) + if (scale < near_0) { // m_main_diag contains eigenvalues m_main_diag.setZero(); @@ -156,16 +158,25 @@ class TridiagEigen int info = 0; // 0 for success, 1 for failure const Scalar considerAsZero = TypeTraits::min(); - const Scalar precision = Scalar(2) * Eigen::NumTraits::epsilon(); + const Scalar precision_inv = Scalar(1) / Eigen::NumTraits::epsilon(); while (end > 0) { for (Index i = start; i < end; i++) - if (abs(subdiag[i]) <= considerAsZero || - abs(subdiag[i]) <= (abs(diag[i]) + abs(diag[i + 1])) * precision) - subdiag[i] = 0; + { + if (abs(subdiag[i]) <= considerAsZero) + subdiag[i] = Scalar(0); + else + { + // abs(subdiag[i]) <= epsilon * sqrt(abs(diag[i]) + abs(diag[i+1])) + // Scaled to prevent underflows. + const Scalar scaled_subdiag = precision_inv * subdiag[i]; + if (scaled_subdiag * scaled_subdiag <= (abs(diag[i]) + abs(diag[i + 1]))) + subdiag[i] = Scalar(0); + } + } - // find the largest unreduced block + // find the largest unreduced block at the end of the matrix. while (end > 0 && subdiag[end - 1] == Scalar(0)) end--; @@ -216,4 +227,4 @@ class TridiagEigen } // namespace Spectra -#endif // TRIDIAG_EIGEN_H +#endif // SPECTRA_TRIDIAG_EIGEN_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergEigen.h b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergEigen.h index 4865e9db8e..f8ceb8ab86 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergEigen.h +++ b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergEigen.h @@ -2,42 +2,41 @@ // // Copyright (C) 2008 Gael Guennebaud // Copyright (C) 2010,2012 Jitse Niesen -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef UPPER_HESSENBERG_EIGEN_H -#define UPPER_HESSENBERG_EIGEN_H +#ifndef SPECTRA_UPPER_HESSENBERG_EIGEN_H +#define SPECTRA_UPPER_HESSENBERG_EIGEN_H #include -#include #include +#include "UpperHessenbergSchur.h" + namespace Spectra { template class UpperHessenbergEigen { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - - typedef Eigen::Ref GenericMatrix; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; - typedef std::complex Complex; - typedef Eigen::Matrix ComplexMatrix; - typedef Eigen::Matrix ComplexVector; + using Complex = std::complex; + using ComplexMatrix = Eigen::Matrix; + using ComplexVector = Eigen::Matrix; Index m_n; // Size of the matrix - Eigen::RealSchur m_realSchur; // Schur decomposition solver + UpperHessenbergSchur m_schur; // Schur decomposition solver Matrix m_matT; // Schur T matrix Matrix m_eivec; // Storing eigenvectors ComplexVector m_eivalues; // Eigenvalues - bool m_computed; void doComputeEigenvectors() @@ -177,7 +176,7 @@ class UpperHessenbergEigen } // Overflow control - Scalar t = std::max(abs(m_matT.coeff(i, n - 1)), abs(m_matT.coeff(i, n))); + Scalar t = (std::max)(abs(m_matT.coeff(i, n - 1)), abs(m_matT.coeff(i, n))); if ((eps * t) * t > Scalar(1)) m_matT.block(i, n - 1, size - i, 2) /= t; } @@ -221,13 +220,9 @@ class UpperHessenbergEigen const Scalar scale = mat.cwiseAbs().maxCoeff(); // Reduce to real Schur form - Matrix Q = Matrix::Identity(m_n, m_n); - m_realSchur.computeFromHessenberg(mat / scale, Q, true); - if (m_realSchur.info() != Eigen::Success) - throw std::runtime_error("UpperHessenbergEigen: eigen decomposition failed"); - - m_matT = m_realSchur.matrixT(); - m_eivec = m_realSchur.matrixU(); + m_schur.compute(mat / scale); + m_schur.swap_T(m_matT); + m_schur.swap_U(m_eivec); // Compute eigenvalues from matT m_eivalues.resize(m_n); @@ -249,7 +244,7 @@ class UpperHessenbergEigen { Scalar t0 = m_matT.coeff(i + 1, i); Scalar t1 = m_matT.coeff(i, i + 1); - Scalar maxval = std::max(abs(p), std::max(abs(t0), abs(t1))); + Scalar maxval = (std::max)(abs(p), (std::max)(abs(t0), abs(t1))); t0 /= maxval; t1 /= maxval; Scalar p0 = p / maxval; @@ -316,4 +311,4 @@ class UpperHessenbergEigen } // namespace Spectra -#endif // UPPER_HESSENBERG_EIGEN_H +#endif // SPECTRA_UPPER_HESSENBERG_EIGEN_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergQR.h b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergQR.h index 5778f11dc6..c685a59a40 100644 --- a/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergQR.h +++ b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergQR.h @@ -1,17 +1,19 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef UPPER_HESSENBERG_QR_H -#define UPPER_HESSENBERG_QR_H +#ifndef SPECTRA_UPPER_HESSENBERG_QR_H +#define SPECTRA_UPPER_HESSENBERG_QR_H #include -#include // std::sqrt -#include // std::fill, std::copy +#include // std::abs, std::sqrt, std::pow +#include // std::fill #include // std::logic_error +#include "../Util/TypeTraits.h" + namespace Spectra { /// @@ -43,16 +45,16 @@ template class UpperHessenbergQR { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Matrix RowVector; - typedef Eigen::Array Array; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using RowVector = Eigen::Matrix; + using Array = Eigen::Array; - typedef Eigen::Ref GenericMatrix; - typedef const Eigen::Ref ConstGenericMatrix; + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; - Matrix m_mat_T; + Matrix m_mat_R; protected: Index m_n; @@ -64,40 +66,107 @@ class UpperHessenbergQR Array m_rot_sin; bool m_computed; + // Given a >= b > 0, compute r = sqrt(a^2 + b^2), c = a / r, and s = b / r with high precision + static void stable_scaling(const Scalar& a, const Scalar& b, Scalar& r, Scalar& c, Scalar& s) + { + using std::sqrt; + using std::pow; + + // Let t = b / a, then 0 < t <= 1 + // c = 1 / sqrt(1 + t^2) + // s = t * c + // r = a * sqrt(1 + t^2) + const Scalar t = b / a; + // We choose a cutoff such that cutoff^4 < eps + // If t > cutoff, use the standard way; otherwise use Taylor series expansion + // to avoid an explicit sqrt() call that may lose precision + const Scalar eps = TypeTraits::epsilon(); + // std::pow() is not constexpr, so we do not declare cutoff to be constexpr + // But most compilers should be able to compute cutoff at compile time + const Scalar cutoff = Scalar(0.1) * pow(eps, Scalar(0.25)); + if (t >= cutoff) + { + const Scalar denom = sqrt(Scalar(1) + t * t); + c = Scalar(1) / denom; + s = t * c; + r = a * denom; + } + else + { + // 1 / sqrt(1 + t^2) ~= 1 - (1/2) * t^2 + (3/8) * t^4 + // 1 / sqrt(1 + l^2) ~= 1 / l - (1/2) / l^3 + (3/8) / l^5 + // == t - (1/2) * t^3 + (3/8) * t^5, where l = 1 / t + // sqrt(1 + t^2) ~= 1 + (1/2) * t^2 - (1/8) * t^4 + (1/16) * t^6 + // + // c = 1 / sqrt(1 + t^2) ~= 1 - t^2 * (1/2 - (3/8) * t^2) + // s = 1 / sqrt(1 + l^2) ~= t * (1 - t^2 * (1/2 - (3/8) * t^2)) + // r = a * sqrt(1 + t^2) ~= a + (1/2) * b * t - (1/8) * b * t^3 + (1/16) * b * t^5 + // == a + (b/2) * t * (1 - t^2 * (1/4 - 1/8 * t^2)) + const Scalar c1 = Scalar(1); + const Scalar c2 = Scalar(0.5); + const Scalar c4 = Scalar(0.25); + const Scalar c8 = Scalar(0.125); + const Scalar c38 = Scalar(0.375); + const Scalar t2 = t * t; + const Scalar tc = t2 * (c2 - c38 * t2); + c = c1 - tc; + s = t - t * tc; + r = a + c2 * b * t * (c1 - t2 * (c4 - c8 * t2)); + + /* const Scalar t_2 = Scalar(0.5) * t; + const Scalar t2_2 = t_2 * t; + const Scalar t3_2 = t2_2 * t; + const Scalar t4_38 = Scalar(1.5) * t2_2 * t2_2; + const Scalar t5_16 = Scalar(0.25) * t3_2 * t2_2; + c = Scalar(1) - t2_2 + t4_38; + s = t - t3_2 + Scalar(6) * t5_16; + r = a + b * (t_2 - Scalar(0.25) * t3_2 + t5_16); */ + } + } + // Given x and y, compute 1) r = sqrt(x^2 + y^2), 2) c = x / r, 3) s = -y / r // If both x and y are zero, set c = 1 and s = 0 // We must implement it in a numerically stable way + // The implementation below is shown to be more accurate than directly computing + // r = std::hypot(x, y); c = x / r; s = -y / r; static void compute_rotation(const Scalar& x, const Scalar& y, Scalar& r, Scalar& c, Scalar& s) { - using std::sqrt; + using std::abs; + + // Only need xsign when x != 0 + const Scalar xsign = (x > Scalar(0)) ? Scalar(1) : Scalar(-1); + const Scalar xabs = abs(x); + if (y == Scalar(0)) + { + c = (x == Scalar(0)) ? Scalar(1) : xsign; + s = Scalar(0); + r = xabs; + return; + } + + // Now we know y != 0 + const Scalar ysign = (y > Scalar(0)) ? Scalar(1) : Scalar(-1); + const Scalar yabs = abs(y); + if (x == Scalar(0)) + { + c = Scalar(0); + s = -ysign; + r = yabs; + return; + } - const Scalar xsign = (x > Scalar(0)) - (x < Scalar(0)); - const Scalar ysign = (y > Scalar(0)) - (y < Scalar(0)); - const Scalar xabs = x * xsign; - const Scalar yabs = y * ysign; + // Now we know x != 0, y != 0 if (xabs > yabs) { - // In this case xabs != 0 - const Scalar ratio = yabs / xabs; // so that 0 <= ratio < 1 - const Scalar common = sqrt(Scalar(1) + ratio * ratio); - c = xsign / common; - r = xabs * common; - s = -y / r; + stable_scaling(xabs, yabs, r, c, s); + c = xsign * c; + s = -ysign * s; } else { - if (yabs == Scalar(0)) - { - r = Scalar(0); - c = Scalar(1); - s = Scalar(0); - return; - } - const Scalar ratio = xabs / yabs; // so that 0 <= ratio <= 1 - const Scalar common = sqrt(Scalar(1) + ratio * ratio); - s = -ysign / common; - r = yabs * common; - c = x / r; + stable_scaling(yabs, xabs, r, s, c); + c = xsign * c; + s = -ysign * s; } } @@ -108,6 +177,7 @@ class UpperHessenbergQR /// UpperHessenbergQR(Index size) : m_n(size), + m_shift(0), m_rot_cos(m_n - 1), m_rot_sin(m_n - 1), m_computed(false) @@ -122,7 +192,7 @@ class UpperHessenbergQR /// \param mat Matrix type can be `Eigen::Matrix` (e.g. /// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). - /// Only the upper triangular and the lower subdiagonal parts of + /// Only the upper triangular and the subdiagonal elements of /// the matrix are used. /// UpperHessenbergQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) : @@ -138,16 +208,16 @@ class UpperHessenbergQR /// /// Virtual destructor. /// - virtual ~UpperHessenbergQR(){}; + virtual ~UpperHessenbergQR() {} /// - /// Conduct the QR factorization of an upper Hessenberg matrix with + /// Compute the QR decomposition of an upper Hessenberg matrix with /// an optional shift. /// /// \param mat Matrix type can be `Eigen::Matrix` (e.g. /// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). - /// Only the upper triangular and the lower subdiagonal parts of + /// Only the upper triangular and the subdiagonal elements of /// the matrix are used. /// virtual void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) @@ -157,54 +227,54 @@ class UpperHessenbergQR throw std::invalid_argument("UpperHessenbergQR: matrix must be square"); m_shift = shift; - m_mat_T.resize(m_n, m_n); + m_mat_R.resize(m_n, m_n); m_rot_cos.resize(m_n - 1); m_rot_sin.resize(m_n - 1); // Make a copy of mat - s * I - std::copy(mat.data(), mat.data() + mat.size(), m_mat_T.data()); - m_mat_T.diagonal().array() -= m_shift; + m_mat_R.noalias() = mat; + m_mat_R.diagonal().array() -= m_shift; Scalar xi, xj, r, c, s; - Scalar *Tii, *ptr; + Scalar *Rii, *ptr; const Index n1 = m_n - 1; for (Index i = 0; i < n1; i++) { - Tii = &m_mat_T.coeffRef(i, i); + Rii = &m_mat_R.coeffRef(i, i); - // Make sure mat_T is upper Hessenberg - // Zero the elements below mat_T(i + 1, i) - std::fill(Tii + 2, Tii + m_n - i, Scalar(0)); + // Make sure R is upper Hessenberg + // Zero the elements below R[i + 1, i] + std::fill(Rii + 2, Rii + m_n - i, Scalar(0)); - xi = Tii[0]; // mat_T(i, i) - xj = Tii[1]; // mat_T(i + 1, i) + xi = Rii[0]; // R[i, i] + xj = Rii[1]; // R[i + 1, i] compute_rotation(xi, xj, r, c, s); - m_rot_cos[i] = c; - m_rot_sin[i] = s; + m_rot_cos.coeffRef(i) = c; + m_rot_sin.coeffRef(i) = s; // For a complete QR decomposition, // we first obtain the rotation matrix // G = [ cos sin] // [-sin cos] - // and then do T[i:(i + 1), i:(n - 1)] = G' * T[i:(i + 1), i:(n - 1)] + // and then do R[i:(i + 1), i:(n - 1)] = G' * R[i:(i + 1), i:(n - 1)] // Gt << c, -s, s, c; - // m_mat_T.block(i, i, 2, m_n - i) = Gt * m_mat_T.block(i, i, 2, m_n - i); - Tii[0] = r; // m_mat_T(i, i) => r - Tii[1] = 0; // m_mat_T(i + 1, i) => 0 - ptr = Tii + m_n; // m_mat_T(i, k), k = i+1, i+2, ..., n-1 + // m_mat_R.block(i, i, 2, m_n - i) = Gt * m_mat_R.block(i, i, 2, m_n - i); + Rii[0] = r; // R[i, i] => r + Rii[1] = 0; // R[i + 1, i] => 0 + ptr = Rii + m_n; // R[i, k], k = i+1, i+2, ..., n-1 for (Index j = i + 1; j < m_n; j++, ptr += m_n) { - Scalar tmp = ptr[0]; + const Scalar tmp = ptr[0]; ptr[0] = c * tmp - s * ptr[1]; ptr[1] = s * tmp + c * ptr[1]; } // If we do not need to calculate the R matrix, then // only the cos and sin sequences are required. - // In such case we only update T[i + 1, (i + 1):(n - 1)] - // m_mat_T.block(i + 1, i + 1, 1, m_n - i - 1) *= c; - // m_mat_T.block(i + 1, i + 1, 1, m_n - i - 1) += s * mat_T.block(i, i + 1, 1, m_n - i - 1); + // In such case we only update R[i + 1, (i + 1):(n - 1)] + // m_mat_R.block(i + 1, i + 1, 1, m_n - i - 1) *= c; + // m_mat_R.block(i + 1, i + 1, 1, m_n - i - 1) += s * m_mat_R.block(i, i + 1, 1, m_n - i - 1); } m_computed = true; @@ -222,7 +292,7 @@ class UpperHessenbergQR if (!m_computed) throw std::logic_error("UpperHessenbergQR: need to call compute() first"); - return m_mat_T; + return m_mat_R; } /// @@ -239,7 +309,7 @@ class UpperHessenbergQR // Make a copy of the R matrix dest.resize(m_n, m_n); - std::copy(m_mat_T.data(), m_mat_T.data() + m_mat_T.size(), dest.data()); + dest.noalias() = m_mat_R; // Compute the RQ matrix const Index n1 = m_n - 1; @@ -252,7 +322,7 @@ class UpperHessenbergQR // [-sin[i] cos[i]] Scalar *Yi, *Yi1; Yi = &dest.coeffRef(0, i); - Yi1 = Yi + m_n; // RQ(0, i + 1) + Yi1 = Yi + m_n; // RQ[0, i + 1] const Index i2 = i + 2; for (Index j = 0; j < i2; j++) { @@ -475,16 +545,23 @@ template class TridiagQR : public UpperHessenbergQR { private: - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef const Eigen::Ref ConstGenericMatrix; - - typedef typename Matrix::Index Index; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using ConstGenericMatrix = const Eigen::Ref; + using ComplexMatrix = Eigen::Matrix, Eigen::Dynamic, Eigen::Dynamic>; + + using UpperHessenbergQR::m_n; + using UpperHessenbergQR::m_shift; + using UpperHessenbergQR::m_rot_cos; + using UpperHessenbergQR::m_rot_sin; + using UpperHessenbergQR::m_computed; Vector m_T_diag; // diagonal elements of T - Vector m_T_lsub; // lower subdiagonal of T - Vector m_T_usub; // upper subdiagonal of T - Vector m_T_usub2; // 2nd upper subdiagonal of T + Vector m_T_subd; // 1st subdiagonal of T + Vector m_R_diag; // diagonal elements of R, where T = QR + Vector m_R_supd; // 1st superdiagonal of R + Vector m_R_supd2; // 2nd superdiagonal of R public: /// @@ -497,15 +574,14 @@ class TridiagQR : public UpperHessenbergQR /// /// Constructor to create an object that performs and stores the - /// QR decomposition of an upper Hessenberg matrix `mat`, with an - /// optional shift: \f$H-sI=QR\f$. Here \f$H\f$ stands for the matrix + /// QR decomposition of a tridiagonal matrix `mat`, with an + /// optional shift: \f$T-sI=QR\f$. Here \f$T\f$ stands for the matrix /// `mat`, and \f$s\f$ is the shift. /// /// \param mat Matrix type can be `Eigen::Matrix` (e.g. /// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). - /// Only the major- and sub- diagonal parts of - /// the matrix are used. + /// Only the diagonal and subdiagonal elements of the matrix are used. /// TridiagQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) : UpperHessenbergQR(mat.rows()) @@ -514,70 +590,84 @@ class TridiagQR : public UpperHessenbergQR } /// - /// Conduct the QR factorization of a tridiagonal matrix with an + /// Compute the QR decomposition of a tridiagonal matrix with an /// optional shift. /// /// \param mat Matrix type can be `Eigen::Matrix` (e.g. /// `Eigen::MatrixXd` and `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). - /// Only the major- and sub- diagonal parts of - /// the matrix are used. + /// Only the diagonal and subdiagonal elements of the matrix are used. /// - void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) + void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) override { - this->m_n = mat.rows(); - if (this->m_n != mat.cols()) + using std::abs; + + m_n = mat.rows(); + if (m_n != mat.cols()) throw std::invalid_argument("TridiagQR: matrix must be square"); - this->m_shift = shift; - m_T_diag.resize(this->m_n); - m_T_lsub.resize(this->m_n - 1); - m_T_usub.resize(this->m_n - 1); - m_T_usub2.resize(this->m_n - 2); - this->m_rot_cos.resize(this->m_n - 1); - this->m_rot_sin.resize(this->m_n - 1); + m_shift = shift; + m_rot_cos.resize(m_n - 1); + m_rot_sin.resize(m_n - 1); + + // Save the diagonal and subdiagonal elements of T + m_T_diag.resize(m_n); + m_T_subd.resize(m_n - 1); + m_T_diag.noalias() = mat.diagonal(); + m_T_subd.noalias() = mat.diagonal(-1); + + // Deflation of small sub-diagonal elements + const Scalar eps = TypeTraits::epsilon(); + for (Index i = 0; i < m_n - 1; i++) + { + if (abs(m_T_subd[i]) <= eps * (abs(m_T_diag[i]) + abs(m_T_diag[i + 1]))) + m_T_subd[i] = Scalar(0); + } - m_T_diag.array() = mat.diagonal().array() - this->m_shift; - m_T_lsub.noalias() = mat.diagonal(-1); - m_T_usub.noalias() = m_T_lsub; + // Apply shift and copy T to R + m_R_diag.resize(m_n); + m_R_supd.resize(m_n - 1); + m_R_supd2.resize(m_n - 2); + m_R_diag.array() = m_T_diag.array() - m_shift; + m_R_supd.noalias() = m_T_subd; // A number of pointers to avoid repeated address calculation - Scalar *c = this->m_rot_cos.data(), // pointer to the cosine vector - *s = this->m_rot_sin.data(), // pointer to the sine vector + Scalar *c = m_rot_cos.data(), // pointer to the cosine vector + *s = m_rot_sin.data(), // pointer to the sine vector r; - const Index n1 = this->m_n - 1; + const Index n1 = m_n - 1, n2 = m_n - 2; for (Index i = 0; i < n1; i++) { - // diag[i] == T[i, i] - // lsub[i] == T[i + 1, i] - // r = sqrt(T[i, i]^2 + T[i + 1, i]^2) - // c = T[i, i] / r, s = -T[i + 1, i] / r - this->compute_rotation(m_T_diag.coeff(i), m_T_lsub.coeff(i), r, *c, *s); + // Rdiag[i] == R[i, i] + // Tsubd[i] == R[i + 1, i] + // r = sqrt(R[i, i]^2 + R[i + 1, i]^2) + // c = R[i, i] / r, s = -R[i + 1, i] / r + this->compute_rotation(m_R_diag.coeff(i), m_T_subd.coeff(i), r, *c, *s); // For a complete QR decomposition, // we first obtain the rotation matrix // G = [ cos sin] // [-sin cos] - // and then do T[i:(i + 1), i:(i + 2)] = G' * T[i:(i + 1), i:(i + 2)] - - // Update T[i, i] and T[i + 1, i] - // The updated value of T[i, i] is known to be r - // The updated value of T[i + 1, i] is known to be 0 - m_T_diag.coeffRef(i) = r; - m_T_lsub.coeffRef(i) = Scalar(0); - // Update T[i, i + 1] and T[i + 1, i + 1] - // usub[i] == T[i, i + 1] - // diag[i + 1] == T[i + 1, i + 1] - const Scalar tmp = m_T_usub.coeff(i); - m_T_usub.coeffRef(i) = (*c) * tmp - (*s) * m_T_diag.coeff(i + 1); - m_T_diag.coeffRef(i + 1) = (*s) * tmp + (*c) * m_T_diag.coeff(i + 1); - // Update T[i, i + 2] and T[i + 1, i + 2] - // usub2[i] == T[i, i + 2] - // usub[i + 1] == T[i + 1, i + 2] - if (i < n1 - 1) + // and then do R[i:(i + 1), i:(i + 2)] = G' * R[i:(i + 1), i:(i + 2)] + + // Update R[i, i] and R[i + 1, i] + // The updated value of R[i, i] is known to be r + // The updated value of R[i + 1, i] is known to be 0 + m_R_diag.coeffRef(i) = r; + // Update R[i, i + 1] and R[i + 1, i + 1] + // Rsupd[i] == R[i, i + 1] + // Rdiag[i + 1] == R[i + 1, i + 1] + const Scalar Tii1 = m_R_supd.coeff(i); + const Scalar Ti1i1 = m_R_diag.coeff(i + 1); + m_R_supd.coeffRef(i) = (*c) * Tii1 - (*s) * Ti1i1; + m_R_diag.coeffRef(i + 1) = (*s) * Tii1 + (*c) * Ti1i1; + // Update R[i, i + 2] and R[i + 1, i + 2] + // Rsupd2[i] == R[i, i + 2] + // Rsupd[i + 1] == R[i + 1, i + 2] + if (i < n2) { - m_T_usub2.coeffRef(i) = -(*s) * m_T_usub.coeff(i + 1); - m_T_usub.coeffRef(i + 1) *= (*c); + m_R_supd2.coeffRef(i) = -(*s) * m_R_supd.coeff(i + 1); + m_R_supd.coeffRef(i + 1) *= (*c); } c++; @@ -585,12 +675,12 @@ class TridiagQR : public UpperHessenbergQR // If we do not need to calculate the R matrix, then // only the cos and sin sequences are required. - // In such case we only update T[i + 1, (i + 1):(i + 2)] - // T[i + 1, i + 1] = c * T[i + 1, i + 1] + s * T[i, i + 1]; - // T[i + 1, i + 2] *= c; + // In such case we only update R[i + 1, (i + 1):(i + 2)] + // R[i + 1, i + 1] = c * R[i + 1, i + 1] + s * R[i, i + 1]; + // R[i + 1, i + 2] *= c; } - this->m_computed = true; + m_computed = true; } /// @@ -600,64 +690,107 @@ class TridiagQR : public UpperHessenbergQR /// \return Returned matrix type will be `Eigen::Matrix`, depending on /// the template parameter `Scalar` defined. /// - Matrix matrix_R() const + Matrix matrix_R() const override { - if (!this->m_computed) + if (!m_computed) throw std::logic_error("TridiagQR: need to call compute() first"); - Matrix R = Matrix::Zero(this->m_n, this->m_n); - R.diagonal().noalias() = m_T_diag; - R.diagonal(1).noalias() = m_T_usub; - R.diagonal(2).noalias() = m_T_usub2; + Matrix R = Matrix::Zero(m_n, m_n); + R.diagonal().noalias() = m_R_diag; + R.diagonal(1).noalias() = m_R_supd; + R.diagonal(2).noalias() = m_R_supd2; return R; } /// - /// Overwrite `dest` with \f$Q'HQ = RQ + sI\f$, where \f$H\f$ is the input matrix `mat`, + /// Overwrite `dest` with \f$Q'TQ = RQ + sI\f$, where \f$T\f$ is the input matrix `mat`, /// and \f$s\f$ is the shift. The result is a tridiagonal matrix. /// /// \param mat The matrix to be overwritten, whose type should be `Eigen::Matrix`, /// depending on the template parameter `Scalar` defined. /// - void matrix_QtHQ(Matrix& dest) const + void matrix_QtHQ(Matrix& dest) const override { - if (!this->m_computed) + using std::abs; + + if (!m_computed) throw std::logic_error("TridiagQR: need to call compute() first"); - // Make a copy of the R matrix - dest.resize(this->m_n, this->m_n); + // In exact arithmetics, Q'TQ = RQ + sI, so we can just apply Q to R and add the shift. + // However, some numerical examples show that this algorithm decreases the precision, + // so we directly apply Q' and Q to T. + + // Copy the saved diagonal and subdiagonal elements of T to `dest` + dest.resize(m_n, m_n); dest.setZero(); dest.diagonal().noalias() = m_T_diag; - // The upper diagonal refers to m_T_usub - // The 2nd upper subdiagonal will be zero in RQ + dest.diagonal(-1).noalias() = m_T_subd; - // Compute the RQ matrix - // [m11 m12] points to RQ[i:(i+1), i:(i+1)] - // [0 m22] + // Ti = [x y 0], Gi = [ cos[i] sin[i] 0], Gi' * Ti * Gi = [x' y' o'] + // [y z w] [-sin[i] cos[i] 0] [y' z' w'] + // [0 w u] [ 0 0 1] [o' w' u'] + // + // x' = c2*x - 2*c*s*y + s2*z + // y' = c*s*(x-z) + (c2-s2)*y + // z' = s2*x + 2*c*s*y + c2*z + // o' = -s*w, w' = c*w, u' = u // - // Gi = [ cos[i] sin[i]] - // [-sin[i] cos[i]] - const Index n1 = this->m_n - 1; + // In iteration (i+1), (y', o') will be further updated to (y'', o''), + // where o'' = 0, y'' = cos[i+1]*y' - sin[i+1]*o' + const Index n1 = m_n - 1, n2 = m_n - 2; for (Index i = 0; i < n1; i++) { - const Scalar c = this->m_rot_cos.coeff(i); - const Scalar s = this->m_rot_sin.coeff(i); - const Scalar m11 = dest.coeff(i, i), - m12 = m_T_usub.coeff(i), - m22 = m_T_diag.coeff(i + 1); + const Scalar c = m_rot_cos.coeff(i); + const Scalar s = m_rot_sin.coeff(i); + const Scalar cs = c * s, c2 = c * c, s2 = s * s; + const Scalar x = dest.coeff(i, i), + y = dest.coeff(i + 1, i), + z = dest.coeff(i + 1, i + 1); + const Scalar c2x = c2 * x, s2x = s2 * x, c2z = c2 * z, s2z = s2 * z; + const Scalar csy2 = Scalar(2) * c * s * y; // Update the diagonal and the lower subdiagonal of dest - dest.coeffRef(i, i) = c * m11 - s * m12; - dest.coeffRef(i + 1, i) = -s * m22; - dest.coeffRef(i + 1, i + 1) = c * m22; + dest.coeffRef(i, i) = c2x - csy2 + s2z; // x' + dest.coeffRef(i + 1, i) = cs * (x - z) + (c2 - s2) * y; // y' + dest.coeffRef(i + 1, i + 1) = s2x + csy2 + c2z; // z' + + if (i < n2) + { + const Scalar ci1 = m_rot_cos.coeff(i + 1); + const Scalar si1 = m_rot_sin.coeff(i + 1); + const Scalar o = -s * m_T_subd.coeff(i + 1); // o' + dest.coeffRef(i + 2, i + 1) *= c; // w' + dest.coeffRef(i + 1, i) = ci1 * dest.coeff(i + 1, i) - si1 * o; // y'' + } + } + + // Deflation of small sub-diagonal elements + const Scalar eps = TypeTraits::epsilon(); + for (Index i = 0; i < n1; i++) + { + const Scalar diag = abs(dest.coeff(i, i)) + abs(dest.coeff(i + 1, i + 1)); + if (abs(dest.coeff(i + 1, i)) <= eps * diag) + dest.coeffRef(i + 1, i) = Scalar(0); } // Copy the lower subdiagonal to upper subdiagonal dest.diagonal(1).noalias() = dest.diagonal(-1); + } - // Add the shift to the diagonal - dest.diagonal().array() += this->m_shift; + /// + /// The version of matrix_QtHQ() when `dest` has a complex value type. + /// + /// This is used in Hermitian eigen solvers where the result is stored + /// as a complex matrix. + /// + void matrix_QtHQ(ComplexMatrix& dest) const + { + // Simply compute the real-typed result and copy to the complex one + Matrix dest_real; + this->matrix_QtHQ(dest_real); + dest.resize(m_n, m_n); + dest.noalias() = dest_real.template cast>(); } }; @@ -667,4 +800,4 @@ class TridiagQR : public UpperHessenbergQR } // namespace Spectra -#endif // UPPER_HESSENBERG_QR_H +#endif // SPECTRA_UPPER_HESSENBERG_QR_H diff --git a/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergSchur.h b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergSchur.h new file mode 100644 index 0000000000..d4be26eb77 --- /dev/null +++ b/gtsam/3rdparty/Spectra/LinAlg/UpperHessenbergSchur.h @@ -0,0 +1,450 @@ +// The code was adapted from Eigen/src/Eigenvaleus/RealSchur.h +// +// Copyright (C) 2008 Gael Guennebaud +// Copyright (C) 2010,2012 Jitse Niesen +// Copyright (C) 2021-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_UPPER_HESSENBERG_SCHUR_H +#define SPECTRA_UPPER_HESSENBERG_SCHUR_H + +#include +#include +#include +#include + +#include "../Util/TypeTraits.h" + +namespace Spectra { + +template +class UpperHessenbergSchur +{ +private: + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using Vector2s = Eigen::Matrix; + using Vector3s = Eigen::Matrix; + using GenericMatrix = Eigen::Ref; + using ConstGenericMatrix = const Eigen::Ref; + + Index m_n; // Size of the matrix + Matrix m_T; // T matrix, A = UTU' + Matrix m_U; // U matrix, A = UTU' + bool m_computed; + + // L1 norm of an upper Hessenberg matrix + static Scalar upper_hessenberg_l1_norm(ConstGenericMatrix& x) + { + const Index n = x.cols(); + Scalar norm(0); + for (Index j = 0; j < n; j++) + norm += x.col(j).segment(0, (std::min)(n, j + 2)).cwiseAbs().sum(); + return norm; + } + + // Look for single small sub-diagonal element and returns its index + Index find_small_subdiag(Index iu, const Scalar& near_0) const + { + using std::abs; + + const Scalar eps = Eigen::NumTraits::epsilon(); + Index res = iu; + while (res > 0) + { + Scalar s = abs(m_T.coeff(res - 1, res - 1)) + abs(m_T.coeff(res, res)); + s = Eigen::numext::maxi(s * eps, near_0); + if (abs(m_T.coeff(res, res - 1)) <= s) + break; + res--; + } + + return res; + } + + // Update T given that rows iu-1 and iu decouple from the rest + void split_off_two_rows(Index iu, const Scalar& ex_shift) + { + using std::sqrt; + using std::abs; + + // The eigenvalues of the 2x2 matrix [a b; c d] are + // trace +/- sqrt(discr/4) where discr = tr^2 - 4*det, tr = a + d, det = ad - bc + Scalar p = Scalar(0.5) * (m_T.coeff(iu - 1, iu - 1) - m_T.coeff(iu, iu)); + Scalar q = p * p + m_T.coeff(iu, iu - 1) * m_T.coeff(iu - 1, iu); // q = tr^2 / 4 - det = discr/4 + m_T.coeffRef(iu, iu) += ex_shift; + m_T.coeffRef(iu - 1, iu - 1) += ex_shift; + + if (q >= Scalar(0)) // Two real eigenvalues + { + Scalar z = sqrt(abs(q)); + Eigen::JacobiRotation rot; + rot.makeGivens((p >= Scalar(0)) ? (p + z) : (p - z), m_T.coeff(iu, iu - 1)); + m_T.rightCols(m_n - iu + 1).applyOnTheLeft(iu - 1, iu, rot.adjoint()); + m_T.topRows(iu + 1).applyOnTheRight(iu - 1, iu, rot); + m_T.coeffRef(iu, iu - 1) = Scalar(0); + m_U.applyOnTheRight(iu - 1, iu, rot); + } + if (iu > 1) + m_T.coeffRef(iu - 1, iu - 2) = Scalar(0); + } + + // Form shift in shift_info, and update ex_shift if an exceptional shift is performed + void compute_shift(Index iu, Index iter, Scalar& ex_shift, Vector3s& shift_info) + { + using std::sqrt; + using std::abs; + + shift_info.coeffRef(0) = m_T.coeff(iu, iu); + shift_info.coeffRef(1) = m_T.coeff(iu - 1, iu - 1); + shift_info.coeffRef(2) = m_T.coeff(iu, iu - 1) * m_T.coeff(iu - 1, iu); + + // Wilkinson's original ad hoc shift + if (iter == 10) + { + ex_shift += shift_info.coeff(0); + for (Index i = 0; i <= iu; ++i) + m_T.coeffRef(i, i) -= shift_info.coeff(0); + Scalar s = abs(m_T.coeff(iu, iu - 1)) + abs(m_T.coeff(iu - 1, iu - 2)); + shift_info.coeffRef(0) = Scalar(0.75) * s; + shift_info.coeffRef(1) = Scalar(0.75) * s; + shift_info.coeffRef(2) = Scalar(-0.4375) * s * s; + } + + // MATLAB's new ad hoc shift + if (iter == 30) + { + Scalar s = (shift_info.coeff(1) - shift_info.coeff(0)) / Scalar(2); + s = s * s + shift_info.coeff(2); + if (s > Scalar(0)) + { + s = sqrt(s); + if (shift_info.coeff(1) < shift_info.coeff(0)) + s = -s; + s = s + (shift_info.coeff(1) - shift_info.coeff(0)) / Scalar(2); + s = shift_info.coeff(0) - shift_info.coeff(2) / s; + ex_shift += s; + for (Index i = 0; i <= iu; ++i) + m_T.coeffRef(i, i) -= s; + shift_info.setConstant(Scalar(0.964)); + } + } + } + + // Compute index im at which Francis QR step starts and the first Householder vector + void init_francis_qr_step(Index il, Index iu, const Vector3s& shift_info, Index& im, Vector3s& first_householder_vec) const + { + using std::abs; + + const Scalar eps = Eigen::NumTraits::epsilon(); + Vector3s& v = first_householder_vec; // alias to save typing + for (im = iu - 2; im >= il; --im) + { + const Scalar Tmm = m_T.coeff(im, im); + const Scalar r = shift_info.coeff(0) - Tmm; + const Scalar s = shift_info.coeff(1) - Tmm; + v.coeffRef(0) = (r * s - shift_info.coeff(2)) / m_T.coeff(im + 1, im) + m_T.coeff(im, im + 1); + v.coeffRef(1) = m_T.coeff(im + 1, im + 1) - Tmm - r - s; + v.coeffRef(2) = m_T.coeff(im + 2, im + 1); + if (im == il) + break; + const Scalar lhs = m_T.coeff(im, im - 1) * (abs(v.coeff(1)) + abs(v.coeff(2))); + const Scalar rhs = v.coeff(0) * (abs(m_T.coeff(im - 1, im - 1)) + abs(Tmm) + abs(m_T.coeff(im + 1, im + 1))); + if (abs(lhs) < eps * rhs) + break; + } + } + + // P = I - tau * v * v' = P' + // PX = X - tau * v * (v'X), X [3 x c] + static void apply_householder_left(const Vector2s& ess, const Scalar& tau, Scalar* x, Index ncol, Index stride) + { + const Scalar v1 = ess.coeff(0), v2 = ess.coeff(1); + const Scalar* const x_end = x + ncol * stride; + for (; x < x_end; x += stride) + { + const Scalar tvx = tau * (x[0] + v1 * x[1] + v2 * x[2]); + x[0] -= tvx; + x[1] -= tvx * v1; + x[2] -= tvx * v2; + } + } + + // P = I - tau * v * v' = P' + // XP = X - tau * (X * v) * v', X [r x 3] + static void apply_householder_right(const Vector2s& ess, const Scalar& tau, Scalar* x, Index nrow, Index stride) + { + const Scalar v1 = ess.coeff(0), v2 = ess.coeff(1); + Scalar* x0 = x; + Scalar* x1 = x + stride; + Scalar* x2 = x1 + stride; + for (Index i = 0; i < nrow; i++) + { + const Scalar txv = tau * (x0[i] + v1 * x1[i] + v2 * x2[i]); + x0[i] -= txv; + x1[i] -= txv * v1; + x2[i] -= txv * v2; + } + } + + // SIMD version of apply_householder_right() + // Inspired by apply_rotation_in_the_plane_selector() in Eigen/src/Jacobi/Jacobi.h + static void apply_householder_right_simd(const Vector2s& ess, const Scalar& tau, Scalar* x, Index nrow, Index stride) + { + // Packet type + using Eigen::internal::ploadu; + using Eigen::internal::pstoreu; + using Eigen::internal::pset1; + using Eigen::internal::padd; + using Eigen::internal::psub; + using Eigen::internal::pmul; + using Packet = typename Eigen::internal::packet_traits::type; + constexpr unsigned char PacketSize = Eigen::internal::packet_traits::size; + constexpr unsigned char Peeling = 2; + constexpr unsigned char Increment = Peeling * PacketSize; + + // Column heads + Scalar* x0 = x; + Scalar* x1 = x + stride; + Scalar* x2 = x1 + stride; + // Pointers for the current row + Scalar* px0 = x0; + Scalar* px1 = x1; + Scalar* px2 = x2; + + // Householder reflectors + const Scalar v1 = ess.coeff(0), v2 = ess.coeff(1); + // Vectorized versions + const Packet vtau = pset1(tau); + const Packet vv1 = pset1(v1); + const Packet vv2 = pset1(v2); + + // n % (2^k) == n & (2^k-1), see https://stackoverflow.com/q/3072665 + // const Index peeling_end = nrow - nrow % Increment; + const Index aligned_end = nrow - (nrow & (PacketSize - 1)); + const Index peeling_end = nrow - (nrow & (Increment - 1)); + for (Index i = 0; i < peeling_end; i += Increment) + { + Packet vx01 = ploadu(px0); + Packet vx02 = ploadu(px0 + PacketSize); + Packet vx11 = ploadu(px1); + Packet vx12 = ploadu(px1 + PacketSize); + Packet vx21 = ploadu(px2); + Packet vx22 = ploadu(px2 + PacketSize); + + // Packet txv1 = vtau * (vx01 + vv1 * vx11 + vv2 * vx21); + Packet txv1 = pmul(vtau, padd(padd(vx01, pmul(vv1, vx11)), pmul(vv2, vx21))); + Packet txv2 = pmul(vtau, padd(padd(vx02, pmul(vv1, vx12)), pmul(vv2, vx22))); + + pstoreu(px0, psub(vx01, txv1)); + pstoreu(px0 + PacketSize, psub(vx02, txv2)); + pstoreu(px1, psub(vx11, pmul(txv1, vv1))); + pstoreu(px1 + PacketSize, psub(vx12, pmul(txv2, vv1))); + pstoreu(px2, psub(vx21, pmul(txv1, vv2))); + pstoreu(px2 + PacketSize, psub(vx22, pmul(txv2, vv2))); + + px0 += Increment; + px1 += Increment; + px2 += Increment; + } + if (aligned_end != peeling_end) + { + px0 = x0 + peeling_end; + px1 = x1 + peeling_end; + px2 = x2 + peeling_end; + + Packet x0_p = ploadu(px0); + Packet x1_p = ploadu(px1); + Packet x2_p = ploadu(px2); + Packet txv = pmul(vtau, padd(padd(x0_p, pmul(vv1, x1_p)), pmul(vv2, x2_p))); + + pstoreu(px0, psub(x0_p, txv)); + pstoreu(px1, psub(x1_p, pmul(txv, vv1))); + pstoreu(px2, psub(x2_p, pmul(txv, vv2))); + } + + // Remaining rows + for (Index i = aligned_end; i < nrow; i++) + { + const Scalar txv = tau * (x0[i] + v1 * x1[i] + v2 * x2[i]); + x0[i] -= txv; + x1[i] -= txv * v1; + x2[i] -= txv * v2; + } + } + + // Perform a Francis QR step involving rows il:iu and columns im:iu + void perform_francis_qr_step(Index il, Index im, Index iu, const Vector3s& first_householder_vec, const Scalar& near_0) + { + using std::abs; + + for (Index k = im; k <= iu - 2; ++k) + { + const bool first_iter = (k == im); + Vector3s v; + if (first_iter) + v = first_householder_vec; + else + v = m_T.template block<3, 1>(k, k - 1); + + Scalar tau, beta; + Vector2s ess; + v.makeHouseholder(ess, tau, beta); + + if (abs(beta) > near_0) // if v is not zero + { + if (first_iter && k > il) + m_T.coeffRef(k, k - 1) = -m_T.coeff(k, k - 1); + else if (!first_iter) + m_T.coeffRef(k, k - 1) = beta; + + // These Householder transformations form the O(n^3) part of the algorithm + // m_T.block(k, k, 3, m_n - k).applyHouseholderOnTheLeft(ess, tau, workspace); + // m_T.block(0, k, (std::min)(iu, k + 3) + 1, 3).applyHouseholderOnTheRight(ess, tau, workspace); + // m_U.block(0, k, m_n, 3).applyHouseholderOnTheRight(ess, tau, workspace); + apply_householder_left(ess, tau, &m_T.coeffRef(k, k), m_n - k, m_n); + apply_householder_right_simd(ess, tau, &m_T.coeffRef(0, k), (std::min)(iu, k + 3) + 1, m_n); + apply_householder_right_simd(ess, tau, &m_U.coeffRef(0, k), m_n, m_n); + } + } + + // The last 2-row block + Eigen::JacobiRotation rot; + Scalar beta; + rot.makeGivens(m_T.coeff(iu - 1, iu - 2), m_T.coeff(iu, iu - 2), &beta); + + if (abs(beta) > near_0) // if v is not zero + { + m_T.coeffRef(iu - 1, iu - 2) = beta; + m_T.rightCols(m_n - iu + 1).applyOnTheLeft(iu - 1, iu, rot.adjoint()); + m_T.topRows(iu + 1).applyOnTheRight(iu - 1, iu, rot); + m_U.applyOnTheRight(iu - 1, iu, rot); + } + + // clean up pollution due to round-off errors + for (Index i = im + 2; i <= iu; ++i) + { + m_T.coeffRef(i, i - 2) = Scalar(0); + if (i > im + 2) + m_T.coeffRef(i, i - 3) = Scalar(0); + } + } + +public: + UpperHessenbergSchur() : + m_n(0), m_computed(false) + {} + + UpperHessenbergSchur(ConstGenericMatrix& mat) : + m_n(mat.rows()), m_computed(false) + { + compute(mat); + } + + void compute(ConstGenericMatrix& mat) + { + using std::abs; + using std::sqrt; + + if (mat.rows() != mat.cols()) + throw std::invalid_argument("UpperHessenbergSchur: matrix must be square"); + + m_n = mat.rows(); + m_T.resize(m_n, m_n); + m_U.resize(m_n, m_n); + constexpr Index max_iter_per_row = 40; + const Index max_iter = m_n * max_iter_per_row; + + m_T.noalias() = mat; + m_U.setIdentity(); + + // The matrix m_T is divided in three parts. + // Rows 0,...,il-1 are decoupled from the rest because m_T(il,il-1) is zero. + // Rows il,...,iu is the part we are working on (the active window). + // Rows iu+1,...,end are already brought in triangular form. + Index iu = m_n - 1; + Index iter = 0; // iteration count for current eigenvalue + Index total_iter = 0; // iteration count for whole matrix + Scalar ex_shift(0); // sum of exceptional shifts + const Scalar norm = upper_hessenberg_l1_norm(m_T); + // sub-diagonal entries smaller than near_0 will be treated as zero. + // We use eps^2 to enable more precision in small eigenvalues. + const Scalar eps = Eigen::NumTraits::epsilon(); + const Scalar near_0 = Eigen::numext::maxi(norm * eps * eps, TypeTraits::min()); + + if (norm != Scalar(0)) + { + while (iu >= 0) + { + Index il = find_small_subdiag(iu, near_0); + + // Check for convergence + if (il == iu) // One root found + { + m_T.coeffRef(iu, iu) += ex_shift; + if (iu > 0) + m_T.coeffRef(iu, iu - 1) = Scalar(0); + iu--; + iter = 0; + } + else if (il == iu - 1) // Two roots found + { + split_off_two_rows(iu, ex_shift); + iu -= 2; + iter = 0; + } + else // No convergence yet + { + Vector3s first_householder_vec = Vector3s::Zero(), shift_info; + compute_shift(iu, iter, ex_shift, shift_info); + iter++; + total_iter++; + if (total_iter > max_iter) + break; + Index im; + init_francis_qr_step(il, iu, shift_info, im, first_householder_vec); + perform_francis_qr_step(il, im, iu, first_householder_vec, near_0); + } + } + } + + if (total_iter > max_iter) + throw std::runtime_error("UpperHessenbergSchur: Schur decomposition failed"); + + m_computed = true; + } + + const Matrix& matrix_T() const + { + if (!m_computed) + throw std::logic_error("UpperHessenbergSchur: need to call compute() first"); + + return m_T; + } + + const Matrix& matrix_U() const + { + if (!m_computed) + throw std::logic_error("UpperHessenbergSchur: need to call compute() first"); + + return m_U; + } + + void swap_T(Matrix& other) + { + m_T.swap(other); + } + + void swap_U(Matrix& other) + { + m_U.swap(other); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_UPPER_HESSENBERG_SCHUR_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h b/gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h index c41cae729e..4e8a03465a 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseCholesky.h @@ -1,15 +1,16 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_CHOLESKY_H -#define DENSE_CHOLESKY_H +#ifndef SPECTRA_DENSE_CHOLESKY_H +#define SPECTRA_DENSE_CHOLESKY_H #include #include #include + #include "../Util/CompInfo.h" namespace Spectra { @@ -22,21 +23,32 @@ namespace Spectra { /// matrix. It is mainly used in the SymGEigsSolver generalized eigen solver /// in the Cholesky decomposition mode. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseCholesky { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstMat; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; const Index m_n; Eigen::LLT m_decomp; - int m_info; // status of the decomposition + CompInfo m_info; // status of the decomposition public: /// @@ -47,16 +59,21 @@ class DenseCholesky /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseCholesky(ConstGenericMatrix& mat) : - m_n(mat.rows()), m_info(NOT_COMPUTED) + template + DenseCholesky(const Eigen::MatrixBase& mat) : + m_n(mat.rows()), m_info(CompInfo::NotComputed) { - if (mat.rows() != mat.cols()) + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseCholesky: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + + if (m_n != mat.cols()) throw std::invalid_argument("DenseCholesky: matrix must be square"); m_decomp.compute(mat); m_info = (m_decomp.info() == Eigen::Success) ? - SUCCESSFUL : - NUMERICAL_ISSUE; + CompInfo::Successful : + CompInfo::NumericalIssue; } /// @@ -72,7 +89,7 @@ class DenseCholesky /// Returns the status of the computation. /// The full list of enumeration values can be found in \ref Enumerations. /// - int info() const { return m_info; } + CompInfo info() const { return m_info; } /// /// Performs the lower triangular solving operation \f$y=L^{-1}x\f$. @@ -105,4 +122,4 @@ class DenseCholesky } // namespace Spectra -#endif // DENSE_CHOLESKY_H +#endif // SPECTRA_DENSE_CHOLESKY_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseGenComplexShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/DenseGenComplexShiftSolve.h index 02ad32f8f9..5c17324a81 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseGenComplexShiftSolve.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseGenComplexShiftSolve.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_GEN_COMPLEX_SHIFT_SOLVE_H -#define DENSE_GEN_COMPLEX_SHIFT_SOLVE_H +#ifndef SPECTRA_DENSE_GEN_COMPLEX_SHIFT_SOLVE_H +#define SPECTRA_DENSE_GEN_COMPLEX_SHIFT_SOLVE_H #include #include @@ -21,27 +21,38 @@ namespace Spectra { /// \f$\sigma\f$ and real-valued vector \f$x\f$. It is mainly used in the /// GenEigsComplexShiftSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseGenComplexShiftSolve { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; - typedef std::complex Complex; - typedef Eigen::Matrix ComplexMatrix; - typedef Eigen::Matrix ComplexVector; + using Complex = std::complex; + using ComplexMatrix = Eigen::Matrix; + using ComplexVector = Eigen::Matrix; - typedef Eigen::PartialPivLU ComplexSolver; + using ComplexSolver = Eigen::PartialPivLU; ConstGenericMatrix m_mat; const Index m_n; ComplexSolver m_solver; - ComplexVector m_x_cache; + mutable ComplexVector m_x_cache; public: /// @@ -52,9 +63,14 @@ class DenseGenComplexShiftSolve /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseGenComplexShiftSolve(ConstGenericMatrix& mat) : + template + DenseGenComplexShiftSolve(const Eigen::MatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseGenComplexShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("DenseGenComplexShiftSolve: matrix must be square"); } @@ -74,7 +90,7 @@ class DenseGenComplexShiftSolve /// \param sigmar Real part of \f$\sigma\f$. /// \param sigmai Imaginary part of \f$\sigma\f$. /// - void set_shift(Scalar sigmar, Scalar sigmai) + void set_shift(const Scalar& sigmar, const Scalar& sigmai) { m_solver.compute(m_mat.template cast() - Complex(sigmar, sigmai) * ComplexMatrix::Identity(m_n, m_n)); m_x_cache.resize(m_n); @@ -89,7 +105,7 @@ class DenseGenComplexShiftSolve /// \param y_out Pointer to the \f$y\f$ vector. /// // y_out = Re( inv(A - sigma * I) * x_in ) - void perform_op(const Scalar* x_in, Scalar* y_out) + void perform_op(const Scalar* x_in, Scalar* y_out) const { m_x_cache.real() = MapConstVec(x_in, m_n); MapVec y(y_out, m_n); @@ -99,4 +115,4 @@ class DenseGenComplexShiftSolve } // namespace Spectra -#endif // DENSE_GEN_COMPLEX_SHIFT_SOLVE_H +#endif // SPECTRA_DENSE_GEN_COMPLEX_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseGenMatProd.h b/gtsam/3rdparty/Spectra/MatOp/DenseGenMatProd.h index c4ade80a36..249cf18e91 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseGenMatProd.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseGenMatProd.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_GEN_MAT_PROD_H -#define DENSE_GEN_MAT_PROD_H +#ifndef SPECTRA_DENSE_GEN_MAT_PROD_H +#define SPECTRA_DENSE_GEN_MAT_PROD_H #include @@ -25,16 +25,27 @@ namespace Spectra { /// \f$x\f$. It is mainly used in the GenEigsSolver and /// SymEigsSolver eigen solvers. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseGenMatProd { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; @@ -47,9 +58,14 @@ class DenseGenMatProd /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseGenMatProd(ConstGenericMatrix& mat) : + template + DenseGenMatProd(const Eigen::MatrixBase& mat) : m_mat(mat) - {} + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseGenMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } /// /// Return the number of rows of the underlying matrix. @@ -73,8 +89,24 @@ class DenseGenMatProd MapVec y(y_out, m_mat.rows()); y.noalias() = m_mat * x; } + + /// + /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. + /// + Matrix operator*(const Eigen::Ref& mat_in) const + { + return m_mat * mat_in; + } + + /// + /// Extract (i,j) element of the underlying matrix. + /// + Scalar operator()(Index i, Index j) const + { + return m_mat(i, j); + } }; } // namespace Spectra -#endif // DENSE_GEN_MAT_PROD_H +#endif // SPECTRA_DENSE_GEN_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseGenRealShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/DenseGenRealShiftSolve.h index 9c93ecc7aa..c7c7d66ad4 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseGenRealShiftSolve.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseGenRealShiftSolve.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_GEN_REAL_SHIFT_SOLVE_H -#define DENSE_GEN_REAL_SHIFT_SOLVE_H +#ifndef SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H +#define SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H #include #include @@ -20,16 +20,27 @@ namespace Spectra { /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and /// vector \f$x\f$. It is mainly used in the GenEigsRealShiftSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseGenRealShiftSolve { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; const Index m_n; @@ -44,9 +55,14 @@ class DenseGenRealShiftSolve /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseGenRealShiftSolve(ConstGenericMatrix& mat) : + template + DenseGenRealShiftSolve(const Eigen::MatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseGenRealShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("DenseGenRealShiftSolve: matrix must be square"); } @@ -63,7 +79,7 @@ class DenseGenRealShiftSolve /// /// Set the real shift \f$\sigma\f$. /// - void set_shift(Scalar sigma) + void set_shift(const Scalar& sigma) { m_solver.compute(m_mat - sigma * Matrix::Identity(m_n, m_n)); } @@ -85,4 +101,4 @@ class DenseGenRealShiftSolve } // namespace Spectra -#endif // DENSE_GEN_REAL_SHIFT_SOLVE_H +#endif // SPECTRA_DENSE_GEN_REAL_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseHermMatProd.h b/gtsam/3rdparty/Spectra/MatOp/DenseHermMatProd.h new file mode 100644 index 0000000000..669f6c6513 --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/DenseHermMatProd.h @@ -0,0 +1,92 @@ +// Copyright (C) 2024-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_DENSE_HERM_MAT_PROD_H +#define SPECTRA_DENSE_HERM_MAT_PROD_H + +#include + +namespace Spectra { + +/// +/// \ingroup MatOp +/// +/// This class defines the matrix-vector multiplication operation on a +/// Hermitian complex matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector +/// \f$x\f$. It is mainly used in the HermEigsSolver eigen solver. +/// +/// \tparam Scalar_ The element type of the matrix, for example, +/// `std::complex`, `std::complex`, +/// and `std::complex`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template +class DenseHermMatProd +{ +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + +private: + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; + + ConstGenericMatrix m_mat; + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param mat An **Eigen** matrix object, whose type can be + /// `Eigen::Matrix` (e.g. `Eigen::MatrixXcd` and + /// `Eigen::MatrixXcf`), or its mapped version + /// (e.g. `Eigen::Map`). + /// + template + DenseHermMatProd(const Eigen::MatrixBase& mat) : + m_mat(mat) + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseHermMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_mat.rows(); } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_mat.cols(); } + + /// + /// Perform the matrix-vector multiplication operation \f$y=Ax\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = A * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + MapConstVec x(x_in, m_mat.cols()); + MapVec y(y_out, m_mat.rows()); + y.noalias() = m_mat.template selfadjointView() * x; + } +}; + +} // namespace Spectra + +#endif // SPECTRA_DENSE_HERM_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseSymMatProd.h b/gtsam/3rdparty/Spectra/MatOp/DenseSymMatProd.h index 76c792686c..d31a14b708 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseSymMatProd.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseSymMatProd.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_SYM_MAT_PROD_H -#define DENSE_SYM_MAT_PROD_H +#ifndef SPECTRA_DENSE_SYM_MAT_PROD_H +#define SPECTRA_DENSE_SYM_MAT_PROD_H #include @@ -18,16 +18,29 @@ namespace Spectra { /// symmetric real matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector /// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseSymMatProd { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; @@ -40,9 +53,14 @@ class DenseSymMatProd /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseSymMatProd(ConstGenericMatrix& mat) : + template + DenseSymMatProd(const Eigen::MatrixBase& mat) : m_mat(mat) - {} + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseSymMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } /// /// Return the number of rows of the underlying matrix. @@ -66,8 +84,24 @@ class DenseSymMatProd MapVec y(y_out, m_mat.rows()); y.noalias() = m_mat.template selfadjointView() * x; } + + /// + /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. + /// + Matrix operator*(const Eigen::Ref& mat_in) const + { + return m_mat.template selfadjointView() * mat_in; + } + + /// + /// Extract (i,j) element of the underlying matrix. + /// + Scalar operator()(Index i, Index j) const + { + return m_mat(i, j); + } }; } // namespace Spectra -#endif // DENSE_SYM_MAT_PROD_H +#endif // SPECTRA_DENSE_SYM_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/DenseSymShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/DenseSymShiftSolve.h index 620aa9413c..77aa191c23 100644 --- a/gtsam/3rdparty/Spectra/MatOp/DenseSymShiftSolve.h +++ b/gtsam/3rdparty/Spectra/MatOp/DenseSymShiftSolve.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef DENSE_SYM_SHIFT_SOLVE_H -#define DENSE_SYM_SHIFT_SOLVE_H +#ifndef SPECTRA_DENSE_SYM_SHIFT_SOLVE_H +#define SPECTRA_DENSE_SYM_SHIFT_SOLVE_H #include #include @@ -22,19 +22,32 @@ namespace Spectra { /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and /// vector \f$x\f$. It is mainly used in the SymEigsShiftSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// +template class DenseSymShiftSolve { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; - const int m_n; + const Index m_n; BKLDLT m_solver; public: @@ -46,10 +59,15 @@ class DenseSymShiftSolve /// `Eigen::MatrixXf`), or its mapped version /// (e.g. `Eigen::Map`). /// - DenseSymShiftSolve(ConstGenericMatrix& mat) : + template + DenseSymShiftSolve(const Eigen::MatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { - if (mat.rows() != mat.cols()) + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(Matrix::IsRowMajor), + "DenseSymShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + + if (m_n != mat.cols()) throw std::invalid_argument("DenseSymShiftSolve: matrix must be square"); } @@ -65,10 +83,10 @@ class DenseSymShiftSolve /// /// Set the real shift \f$\sigma\f$. /// - void set_shift(Scalar sigma) + void set_shift(const Scalar& sigma) { m_solver.compute(m_mat, Uplo, sigma); - if (m_solver.info() != SUCCESSFUL) + if (m_solver.info() != CompInfo::Successful) throw std::invalid_argument("DenseSymShiftSolve: factorization failed with the given shift"); } @@ -89,4 +107,4 @@ class DenseSymShiftSolve } // namespace Spectra -#endif // DENSE_SYM_SHIFT_SOLVE_H +#endif // SPECTRA_DENSE_SYM_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h b/gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h index a73efc3894..e2e4bc88b8 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseCholesky.h @@ -1,16 +1,17 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_CHOLESKY_H -#define SPARSE_CHOLESKY_H +#ifndef SPECTRA_SPARSE_CHOLESKY_H +#define SPECTRA_SPARSE_CHOLESKY_H #include #include #include #include + #include "../Util/CompInfo.h" namespace Spectra { @@ -23,20 +24,33 @@ namespace Spectra { /// matrix. It is mainly used in the SymGEigsSolver generalized eigen solver /// in the Cholesky decomposition mode. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseCholesky { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using SparseMatrix = Eigen::SparseMatrix; const Index m_n; Eigen::SimplicialLLT m_decomp; - int m_info; // status of the decomposition + CompInfo m_info; // status of the decomposition public: /// @@ -46,16 +60,21 @@ class SparseCholesky /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseCholesky(ConstGenericSparseMatrix& mat) : + template + SparseCholesky(const Eigen::SparseMatrixBase& mat) : m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseCholesky: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("SparseCholesky: matrix must be square"); m_decomp.compute(mat); m_info = (m_decomp.info() == Eigen::Success) ? - SUCCESSFUL : - NUMERICAL_ISSUE; + CompInfo::Successful : + CompInfo::NumericalIssue; } /// @@ -71,7 +90,7 @@ class SparseCholesky /// Returns the status of the computation. /// The full list of enumeration values can be found in \ref Enumerations. /// - int info() const { return m_info; } + CompInfo info() const { return m_info; } /// /// Performs the lower triangular solving operation \f$y=L^{-1}x\f$. @@ -106,4 +125,4 @@ class SparseCholesky } // namespace Spectra -#endif // SPARSE_CHOLESKY_H +#endif // SPECTRA_SPARSE_CHOLESKY_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseGenComplexShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/SparseGenComplexShiftSolve.h new file mode 100644 index 0000000000..5ead9e6b68 --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/SparseGenComplexShiftSolve.h @@ -0,0 +1,124 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SPARSE_GEN_COMPLEX_SHIFT_SOLVE_H +#define SPECTRA_SPARSE_GEN_COMPLEX_SHIFT_SOLVE_H + +#include +#include +#include +#include + +namespace Spectra { + +/// +/// \ingroup MatOp +/// +/// This class defines the complex shift-solve operation on a sparse real matrix \f$A\f$, +/// i.e., calculating \f$y=\mathrm{Re}\{(A-\sigma I)^{-1}x\}\f$ for any complex-valued +/// \f$\sigma\f$ and real-valued vector \f$x\f$. It is mainly used in the +/// GenEigsComplexShiftSolver eigen solver. +/// +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template +class SparseGenComplexShiftSolve +{ +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + +private: + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; + + using Complex = std::complex; + using ComplexVector = Eigen::Matrix; + using SparseComplexMatrix = Eigen::SparseMatrix; + + using ComplexSolver = Eigen::SparseLU; + + ConstGenericSparseMatrix m_mat; + const Index m_n; + ComplexSolver m_solver; + mutable ComplexVector m_x_cache; + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param mat An **Eigen** sparse matrix object, whose type can be + /// `Eigen::SparseMatrix` or its mapped version + /// `Eigen::Map >`. + /// + template + SparseGenComplexShiftSolve(const Eigen::SparseMatrixBase& mat) : + m_mat(mat), m_n(mat.rows()) + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseGenComplexShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + + if (mat.rows() != mat.cols()) + throw std::invalid_argument("SparseGenComplexShiftSolve: matrix must be square"); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_n; } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_n; } + + /// + /// Set the complex shift \f$\sigma\f$. + /// + /// \param sigmar Real part of \f$\sigma\f$. + /// \param sigmai Imaginary part of \f$\sigma\f$. + /// + void set_shift(const Scalar& sigmar, const Scalar& sigmai) + { + // Create a sparse idendity matrix (1 + 0i on diagonal) + SparseComplexMatrix I(m_n, m_n); + I.setIdentity(); + // Sparse LU decomposition + m_solver.compute(m_mat.template cast() - Complex(sigmar, sigmai) * I); + // Set cache to zero + m_x_cache.resize(m_n); + m_x_cache.setZero(); + } + + /// + /// Perform the complex shift-solve operation + /// \f$y=\mathrm{Re}\{(A-\sigma I)^{-1}x\}\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = Re( inv(A - sigma * I) * x_in ) + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + m_x_cache.real() = MapConstVec(x_in, m_n); + MapVec y(y_out, m_n); + y.noalias() = m_solver.solve(m_x_cache).real(); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_SPARSE_GEN_COMPLEX_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseGenMatProd.h b/gtsam/3rdparty/Spectra/MatOp/SparseGenMatProd.h index 0cc1f6674e..b65cbc6cf8 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseGenMatProd.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseGenMatProd.h @@ -1,17 +1,16 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_GEN_MAT_PROD_H -#define SPARSE_GEN_MAT_PROD_H +#ifndef SPECTRA_SPARSE_GEN_MAT_PROD_H +#define SPECTRA_SPARSE_GEN_MAT_PROD_H #include #include namespace Spectra { - /// /// \ingroup MatOp /// @@ -20,16 +19,29 @@ namespace Spectra { /// \f$x\f$. It is mainly used in the GenEigsSolver and SymEigsSolver /// eigen solvers. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseGenMatProd { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using Matrix = Eigen::Matrix; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; ConstGenericSparseMatrix m_mat; @@ -41,9 +53,14 @@ class SparseGenMatProd /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseGenMatProd(ConstGenericSparseMatrix& mat) : + template + SparseGenMatProd(const Eigen::SparseMatrixBase& mat) : m_mat(mat) - {} + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseGenMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } /// /// Return the number of rows of the underlying matrix. @@ -67,8 +84,24 @@ class SparseGenMatProd MapVec y(y_out, m_mat.rows()); y.noalias() = m_mat * x; } + + /// + /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. + /// + Matrix operator*(const Eigen::Ref& mat_in) const + { + return m_mat * mat_in; + } + + /// + /// Extract (i,j) element of the underlying matrix. + /// + Scalar operator()(Index i, Index j) const + { + return m_mat.coeff(i, j); + } }; } // namespace Spectra -#endif // SPARSE_GEN_MAT_PROD_H +#endif // SPECTRA_SPARSE_GEN_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseGenRealShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/SparseGenRealShiftSolve.h index fc6e3c508e..f89376fd92 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseGenRealShiftSolve.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseGenRealShiftSolve.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_GEN_REAL_SHIFT_SOLVE_H -#define SPARSE_GEN_REAL_SHIFT_SOLVE_H +#ifndef SPECTRA_SPARSE_GEN_REAL_SHIFT_SOLVE_H +#define SPECTRA_SPARSE_GEN_REAL_SHIFT_SOLVE_H #include #include @@ -21,19 +21,31 @@ namespace Spectra { /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and /// vector \f$x\f$. It is mainly used in the GenEigsRealShiftSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseGenRealShiftSolve { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; ConstGenericSparseMatrix m_mat; - const int m_n; + const Index m_n; Eigen::SparseLU m_solver; public: @@ -44,9 +56,14 @@ class SparseGenRealShiftSolve /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseGenRealShiftSolve(ConstGenericSparseMatrix& mat) : + template + SparseGenRealShiftSolve(const Eigen::SparseMatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseGenRealShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("SparseGenRealShiftSolve: matrix must be square"); } @@ -63,7 +80,7 @@ class SparseGenRealShiftSolve /// /// Set the real shift \f$\sigma\f$. /// - void set_shift(Scalar sigma) + void set_shift(const Scalar& sigma) { SparseMatrix I(m_n, m_n); I.setIdentity(); @@ -90,4 +107,4 @@ class SparseGenRealShiftSolve } // namespace Spectra -#endif // SPARSE_GEN_REAL_SHIFT_SOLVE_H +#endif // SPECTRA_SPARSE_GEN_REAL_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseHermMatProd.h b/gtsam/3rdparty/Spectra/MatOp/SparseHermMatProd.h new file mode 100644 index 0000000000..47d8493631 --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/SparseHermMatProd.h @@ -0,0 +1,92 @@ +// Copyright (C) 2024-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SPARSE_HERM_MAT_PROD_H +#define SPECTRA_SPARSE_HERM_MAT_PROD_H + +#include +#include + +namespace Spectra { + +/// +/// \ingroup MatOp +/// +/// This class defines the matrix-vector multiplication operation on a +/// sparse real symmetric matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector +/// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver. +/// +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template +class SparseHermMatProd +{ +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + +private: + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using Matrix = Eigen::Matrix; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; + + ConstGenericSparseMatrix m_mat; + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param mat An **Eigen** sparse matrix object, whose type can be + /// `Eigen::SparseMatrix` or its mapped version + /// `Eigen::Map >`. + /// + template + SparseHermMatProd(const Eigen::SparseMatrixBase& mat) : + m_mat(mat) + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseHermMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_mat.rows(); } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_mat.cols(); } + + /// + /// Perform the matrix-vector multiplication operation \f$y=Ax\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = A * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + MapConstVec x(x_in, m_mat.cols()); + MapVec y(y_out, m_mat.rows()); + y.noalias() = m_mat.template selfadjointView() * x; + } +}; +} // namespace Spectra + +#endif // SPECTRA_SPARSE_HERM_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseRegularInverse.h b/gtsam/3rdparty/Spectra/MatOp/SparseRegularInverse.h index 3abd0c177f..03562a86b3 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseRegularInverse.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseRegularInverse.h @@ -1,11 +1,11 @@ -// Copyright (C) 2017-2019 Yixuan Qiu +// Copyright (C) 2017-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_REGULAR_INVERSE_H -#define SPARSE_REGULAR_INVERSE_H +#ifndef SPECTRA_SPARSE_REGULAR_INVERSE_H +#define SPECTRA_SPARSE_REGULAR_INVERSE_H #include #include @@ -25,20 +25,35 @@ namespace Spectra { /// This class is intended to be used with the SymGEigsSolver generalized eigen solver /// in the regular inverse mode. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseRegularInverse { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; ConstGenericSparseMatrix m_mat; - const int m_n; + const Index m_n; Eigen::ConjugateGradient m_cg; + mutable CompInfo m_info; public: /// @@ -48,13 +63,21 @@ class SparseRegularInverse /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseRegularInverse(ConstGenericSparseMatrix& mat) : + template + SparseRegularInverse(const Eigen::SparseMatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseRegularInverse: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("SparseRegularInverse: matrix must be square"); m_cg.compute(mat); + m_info = (m_cg.info() == Eigen::Success) ? + CompInfo::Successful : + CompInfo::NumericalIssue; } /// @@ -66,6 +89,12 @@ class SparseRegularInverse /// Index cols() const { return m_n; } + /// + /// Returns the status of the computation. + /// The full list of enumeration values can be found in \ref Enumerations. + /// + CompInfo info() const { return m_info; } + /// /// Perform the solving operation \f$y=B^{-1}x\f$. /// @@ -78,6 +107,12 @@ class SparseRegularInverse MapConstVec x(x_in, m_n); MapVec y(y_out, m_n); y.noalias() = m_cg.solve(x); + + m_info = (m_cg.info() == Eigen::Success) ? + CompInfo::Successful : + CompInfo::NotConverging; + if (m_info != CompInfo::Successful) + throw std::runtime_error("SparseRegularInverse: CG solver does not converge"); } /// @@ -87,7 +122,7 @@ class SparseRegularInverse /// \param y_out Pointer to the \f$y\f$ vector. /// // y_out = B * x_in - void mat_prod(const Scalar* x_in, Scalar* y_out) const + void perform_op(const Scalar* x_in, Scalar* y_out) const { MapConstVec x(x_in, m_n); MapVec y(y_out, m_n); @@ -97,4 +132,4 @@ class SparseRegularInverse } // namespace Spectra -#endif // SPARSE_REGULAR_INVERSE_H +#endif // SPECTRA_SPARSE_REGULAR_INVERSE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseSymMatProd.h b/gtsam/3rdparty/Spectra/MatOp/SparseSymMatProd.h index 2dd8799eb4..9ac47846d6 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseSymMatProd.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseSymMatProd.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_SYM_MAT_PROD_H -#define SPARSE_SYM_MAT_PROD_H +#ifndef SPECTRA_SPARSE_SYM_MAT_PROD_H +#define SPECTRA_SPARSE_SYM_MAT_PROD_H #include #include @@ -19,16 +19,31 @@ namespace Spectra { /// sparse real symmetric matrix \f$A\f$, i.e., calculating \f$y=Ax\f$ for any vector /// \f$x\f$. It is mainly used in the SymEigsSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseSymMatProd { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using Matrix = Eigen::Matrix; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; ConstGenericSparseMatrix m_mat; @@ -40,9 +55,14 @@ class SparseSymMatProd /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseSymMatProd(ConstGenericSparseMatrix& mat) : + template + SparseSymMatProd(const Eigen::SparseMatrixBase& mat) : m_mat(mat) - {} + { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseSymMatProd: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + } /// /// Return the number of rows of the underlying matrix. @@ -66,8 +86,23 @@ class SparseSymMatProd MapVec y(y_out, m_mat.rows()); y.noalias() = m_mat.template selfadjointView() * x; } -}; + /// + /// Perform the matrix-matrix multiplication operation \f$y=Ax\f$. + /// + Matrix operator*(const Eigen::Ref& mat_in) const + { + return m_mat.template selfadjointView() * mat_in; + } + + /// + /// Extract (i,j) element of the underlying matrix. + /// + Scalar operator()(Index i, Index j) const + { + return m_mat.coeff(i, j); + } +}; } // namespace Spectra -#endif // SPARSE_SYM_MAT_PROD_H +#endif // SPECTRA_SPARSE_SYM_MAT_PROD_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SparseSymShiftSolve.h b/gtsam/3rdparty/Spectra/MatOp/SparseSymShiftSolve.h index 674c6ac523..ddf3194770 100644 --- a/gtsam/3rdparty/Spectra/MatOp/SparseSymShiftSolve.h +++ b/gtsam/3rdparty/Spectra/MatOp/SparseSymShiftSolve.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SPARSE_SYM_SHIFT_SOLVE_H -#define SPARSE_SYM_SHIFT_SOLVE_H +#ifndef SPECTRA_SPARSE_SYM_SHIFT_SOLVE_H +#define SPECTRA_SPARSE_SYM_SHIFT_SOLVE_H #include #include @@ -21,19 +21,33 @@ namespace Spectra { /// i.e., calculating \f$y=(A-\sigma I)^{-1}x\f$ for any real \f$\sigma\f$ and /// vector \f$x\f$. It is mainly used in the SymEigsShiftSolver eigen solver. /// -template +/// \tparam Scalar_ The element type of the matrix, for example, +/// `float`, `double`, and `long double`. +/// \tparam Uplo Either `Eigen::Lower` or `Eigen::Upper`, indicating which +/// triangular part of the matrix is used. +/// \tparam Flags Either `Eigen::ColMajor` or `Eigen::RowMajor`, indicating +/// the storage format of the input matrix. +/// \tparam StorageIndex The type of the indices for the sparse matrix. +/// +template class SparseSymShiftSolve { +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef Eigen::SparseMatrix SparseMatrix; - typedef const Eigen::Ref ConstGenericSparseMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using SparseMatrix = Eigen::SparseMatrix; + using ConstGenericSparseMatrix = const Eigen::Ref; ConstGenericSparseMatrix m_mat; - const int m_n; + const Index m_n; Eigen::SparseLU m_solver; public: @@ -44,9 +58,14 @@ class SparseSymShiftSolve /// `Eigen::SparseMatrix` or its mapped version /// `Eigen::Map >`. /// - SparseSymShiftSolve(ConstGenericSparseMatrix& mat) : + template + SparseSymShiftSolve(const Eigen::SparseMatrixBase& mat) : m_mat(mat), m_n(mat.rows()) { + static_assert( + static_cast(Derived::PlainObject::IsRowMajor) == static_cast(SparseMatrix::IsRowMajor), + "SparseSymShiftSolve: the \"Flags\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + if (mat.rows() != mat.cols()) throw std::invalid_argument("SparseSymShiftSolve: matrix must be square"); } @@ -63,7 +82,7 @@ class SparseSymShiftSolve /// /// Set the real shift \f$\sigma\f$. /// - void set_shift(Scalar sigma) + void set_shift(const Scalar& sigma) { SparseMatrix mat = m_mat.template selfadjointView(); SparseMatrix identity(m_n, m_n); @@ -92,4 +111,4 @@ class SparseSymShiftSolve } // namespace Spectra -#endif // SPARSE_SYM_SHIFT_SOLVE_H +#endif // SPECTRA_SPARSE_SYM_SHIFT_SOLVE_H diff --git a/gtsam/3rdparty/Spectra/MatOp/SymShiftInvert.h b/gtsam/3rdparty/Spectra/MatOp/SymShiftInvert.h new file mode 100644 index 0000000000..2b6eacce7e --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/SymShiftInvert.h @@ -0,0 +1,245 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SYM_SHIFT_INVERT_H +#define SPECTRA_SYM_SHIFT_INVERT_H + +#include +#include +#include +#include +#include // std::conditional, std::is_same + +#include "../LinAlg/BKLDLT.h" +#include "../Util/CompInfo.h" + +namespace Spectra { + +/// \cond + +// Compute and factorize A-sigma*B without unnecessary copying +// Default case: A is sparse, B is sparse +template +class SymShiftInvertHelper +{ +public: + template + static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma) + { + using SpMat = typename ArgA::PlainObject; + SpMat matA = A.template selfadjointView(); + SpMat matB = B.template selfadjointView(); + SpMat mat = matA - sigma * matB; + // SparseLU solver + fac.isSymmetric(true); + fac.compute(mat); + // Return true if successful + return fac.info() == Eigen::Success; + } +}; + +// A is dense, B is dense or sparse +template +class SymShiftInvertHelper +{ +public: + template + static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma) + { + using Matrix = typename ArgA::PlainObject; + // Make a copy of the triangular part of A + Matrix mat(A.rows(), A.cols()); + mat.template triangularView() = A; + // Update triangular part of mat + if (UploA == UploB) + mat -= (B * sigma).template triangularView(); + else + mat -= (B * sigma).template triangularView().transpose(); + // BKLDLT solver + fac.compute(mat, UploA); + // Return true if successful + return fac.info() == CompInfo::Successful; + } +}; + +// A is sparse, B is dense +template +class SymShiftInvertHelper +{ +public: + template + static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma) + { + using Matrix = typename ArgB::PlainObject; + // Construct the triangular part of -sigma*B + Matrix mat(B.rows(), B.cols()); + mat.template triangularView() = -sigma * B; + // Update triangular part of mat + if (UploA == UploB) + mat += A.template triangularView(); + else + mat += A.template triangularView().transpose(); + // BKLDLT solver + fac.compute(mat, UploB); + // Return true if successful + return fac.info() == CompInfo::Successful; + } +}; + +/// \endcond + +/// +/// \ingroup MatOp +/// +/// This class defines matrix operations required by the generalized eigen solver +/// in the shift-and-invert mode. Given two symmetric matrices \f$A\f$ and \f$B\f$, +/// it solves the linear equation \f$y=(A-\sigma B)^{-1}x\f$, where \f$\sigma\f$ is a real shift. +/// Each of \f$A\f$ and \f$B\f$ can be a dense or sparse matrix. +/// +/// This class is intended to be used with the SymGEigsShiftSolver generalized eigen solver. +/// +/// \tparam Scalar_ The element type of the matrices. +/// Currently supported types are `float`, `double`, and `long double`. +/// \tparam TypeA The type of the \f$A\f$ matrix, indicating whether \f$A\f$ is +/// dense or sparse. Possible values are `Eigen::Dense` and `Eigen::Sparse`. +/// \tparam TypeB The type of the \f$B\f$ matrix, indicating whether \f$B\f$ is +/// dense or sparse. Possible values are `Eigen::Dense` and `Eigen::Sparse`. +/// \tparam UploA Whether the lower or upper triangular part of \f$A\f$ should be used. +/// Possible values are `Eigen::Lower` and `Eigen::Upper`. +/// \tparam UploB Whether the lower or upper triangular part of \f$B\f$ should be used. +/// Possible values are `Eigen::Lower` and `Eigen::Upper`. +/// \tparam FlagsA Additional flags for the matrix class of \f$A\f$. +/// Possible values are `Eigen::ColMajor` and `Eigen::RowMajor`. +/// \tparam FlagsB Additional flags for the matrix class of \f$B\f$. +/// Possible values are `Eigen::ColMajor` and `Eigen::RowMajor`. +/// \tparam StorageIndexA The storage index type of the \f$A\f$ matrix, only used when \f$A\f$ +/// is a sparse matrix. +/// \tparam StorageIndexB The storage index type of the \f$B\f$ matrix, only used when \f$B\f$ +/// is a sparse matrix. +/// +template +class SymShiftInvert +{ +public: + /// + /// Element type of the matrix. + /// + using Scalar = Scalar_; + +private: + using Index = Eigen::Index; + + // Hypothetical type of the A matrix, either dense or sparse + using DenseTypeA = Eigen::Matrix; + using SparseTypeA = Eigen::SparseMatrix; + // Whether A is sparse + using ASparse = std::is_same; + // Actual type of the A matrix + using MatrixA = typename std::conditional::type; + + // Hypothetical type of the B matrix, either dense or sparse + using DenseTypeB = Eigen::Matrix; + using SparseTypeB = Eigen::SparseMatrix; + // Whether B is sparse + using BSparse = std::is_same; + // Actual type of the B matrix + using MatrixB = typename std::conditional::type; + + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + + // The type of A-sigma*B if one of A and B is dense + // DenseType = if (A is dense) MatrixA else MatrixB + using DenseType = typename std::conditional::type; + // The type of A-sigma*B + // If both A and B are sparse, the result is MatrixA; otherwise the result is DenseType + using ResType = typename std::conditional::type; + + // If both A and B are sparse, then the result A-sigma*B is sparse, so we use + // sparseLU for factorization; otherwise A-sigma*B is dense, and we use BKLDLT + using FacType = typename std::conditional< + ASparse::value && BSparse::value, + Eigen::SparseLU, + BKLDLT>::type; + + using ConstGenericMatrixA = const Eigen::Ref; + using ConstGenericMatrixB = const Eigen::Ref; + + ConstGenericMatrixA m_matA; + ConstGenericMatrixB m_matB; + const Index m_n; + FacType m_solver; + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param A A dense or sparse matrix object, whose type can be `Eigen::Matrix<...>`, + /// `Eigen::SparseMatrix<...>`, `Eigen::Map>`, + /// `Eigen::Map>`, `Eigen::Ref>`, + /// `Eigen::Ref>`, etc. + /// \param B A dense or sparse matrix object. + /// + template + SymShiftInvert(const Eigen::EigenBase& A, const Eigen::EigenBase& B) : + m_matA(A.derived()), m_matB(B.derived()), m_n(A.rows()) + { + static_assert( + static_cast(DerivedA::PlainObject::IsRowMajor) == static_cast(MatrixA::IsRowMajor), + "SymShiftInvert: the \"FlagsA\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + + static_assert( + static_cast(DerivedB::PlainObject::IsRowMajor) == static_cast(MatrixB::IsRowMajor), + "SymShiftInvert: the \"FlagsB\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)"); + + if (m_n != A.cols() || m_n != B.rows() || m_n != B.cols()) + throw std::invalid_argument("SymShiftInvert: A and B must be square matrices of the same size"); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_n; } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_n; } + + /// + /// Set the real shift \f$\sigma\f$. + /// + void set_shift(const Scalar& sigma) + { + constexpr bool AIsSparse = ASparse::value; + constexpr bool BIsSparse = BSparse::value; + using Helper = SymShiftInvertHelper; + const bool success = Helper::factorize(m_solver, m_matA, m_matB, sigma); + if (!success) + throw std::invalid_argument("SymShiftInvert: factorization failed with the given shift"); + } + + /// + /// Perform the shift-invert operation \f$y=(A-\sigma B)^{-1}x\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = inv(A - sigma * B) * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + MapConstVec x(x_in, m_n); + MapVec y(y_out, m_n); + y.noalias() = m_solver.solve(x); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_SYM_SHIFT_INVERT_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/ArnoldiOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/ArnoldiOp.h index 68654aafdd..0188f6b196 100644 --- a/gtsam/3rdparty/Spectra/MatOp/internal/ArnoldiOp.h +++ b/gtsam/3rdparty/Spectra/MatOp/internal/ArnoldiOp.h @@ -1,14 +1,15 @@ -// Copyright (C) 2018-2019 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef ARNOLDI_OP_H -#define ARNOLDI_OP_H +#ifndef SPECTRA_ARNOLDI_OP_H +#define SPECTRA_ARNOLDI_OP_H #include -#include // std::sqrt +#include // std::sqrt +#include // std::real namespace Spectra { @@ -32,51 +33,62 @@ template class ArnoldiOp { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; - OpType& m_op; - BOpType& m_Bop; - Vector m_cache; + const OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; public: - ArnoldiOp(OpType* op, BOpType* Bop) : - m_op(*op), m_Bop(*Bop), m_cache(op->rows()) + ArnoldiOp(const OpType& op, const BOpType& Bop) : + m_op(op), m_Bop(Bop), m_cache(op.rows()) {} + // Move constructor + ArnoldiOp(ArnoldiOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + inline Index rows() const { return m_op.rows(); } - // In generalized eigenvalue problem Ax=lambda*Bx, define the inner product to be = x'By. - // For regular eigenvalue problems, it is the usual inner product = x'y + // In generalized eigenvalue problem Ax=lambda*Bx, define the inner product to be = (x^H)By. + // For regular eigenvalue problems, it is the usual inner product = (x^H)y - // Compute = x'By + // Compute = (x^H)By // x and y are two vectors template - Scalar inner_product(const Arg1& x, const Arg2& y) + Scalar inner_product(const Arg1& x, const Arg2& y) const { - m_Bop.mat_prod(y.data(), m_cache.data()); + m_Bop.perform_op(y.data(), m_cache.data()); return x.dot(m_cache); } - // Compute res = = X'By + // Compute res = = (X^H)By // X is a matrix, y is a vector, res is a vector template - void trans_product(const Arg1& x, const Arg2& y, Eigen::Ref res) + void adjoint_product(const Arg1& x, const Arg2& y, Eigen::Ref res) const { - m_Bop.mat_prod(y.data(), m_cache.data()); - res.noalias() = x.transpose() * m_cache; + m_Bop.perform_op(y.data(), m_cache.data()); + res.noalias() = x.adjoint() * m_cache; } - // B-norm of a vector, ||x||_B = sqrt(x'Bx) + // B-norm of a vector, ||x||_B = sqrt((x^H)Bx) template - Scalar norm(const Arg& x) + RealScalar norm(const Arg& x) const { using std::sqrt; - return sqrt(inner_product(x, x)); + using std::real; + return sqrt(real(inner_product(x, x))); } // The "A" operator to generate the Krylov subspace - inline void perform_op(const Scalar* x_in, Scalar* y_out) + inline void perform_op(const Scalar* x_in, Scalar* y_out) const { m_op.perform_op(x_in, y_out); } @@ -99,19 +111,21 @@ template class ArnoldiOp { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; - OpType& m_op; + const OpType& m_op; public: - ArnoldiOp(OpType* op, IdentityBOp* /*Bop*/) : - m_op(*op) + ArnoldiOp(const OpType& op, const IdentityBOp& /*Bop*/) : + m_op(op) {} inline Index rows() const { return m_op.rows(); } - // Compute = x'y + // Compute = (x^H)y // x and y are two vectors template Scalar inner_product(const Arg1& x, const Arg2& y) const @@ -119,23 +133,23 @@ class ArnoldiOp return x.dot(y); } - // Compute res = = X'y + // Compute res = = (X^H)y // X is a matrix, y is a vector, res is a vector template - void trans_product(const Arg1& x, const Arg2& y, Eigen::Ref res) const + void adjoint_product(const Arg1& x, const Arg2& y, Eigen::Ref res) const { - res.noalias() = x.transpose() * y; + res.noalias() = x.adjoint() * y; } // B-norm of a vector. For regular eigenvalue problems it is simply the L2 norm template - Scalar norm(const Arg& x) + RealScalar norm(const Arg& x) const { return x.norm(); } // The "A" operator to generate the Krylov subspace - inline void perform_op(const Scalar* x_in, Scalar* y_out) + inline void perform_op(const Scalar* x_in, Scalar* y_out) const { m_op.perform_op(x_in, y_out); } @@ -147,4 +161,4 @@ class ArnoldiOp } // namespace Spectra -#endif // ARNOLDI_OP_H +#endif // SPECTRA_ARNOLDI_OP_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsBucklingOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsBucklingOp.h new file mode 100644 index 0000000000..b3803818c7 --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsBucklingOp.h @@ -0,0 +1,95 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SYM_GEIGS_BUCKLING_OP_H +#define SPECTRA_SYM_GEIGS_BUCKLING_OP_H + +#include + +#include "../SymShiftInvert.h" +#include "../SparseSymMatProd.h" + +namespace Spectra { + +/// +/// \ingroup Operators +/// +/// This class defines the matrix operation for generalized eigen solver in the +/// buckling mode. It computes \f$y=(K-\sigma K_G)^{-1}Kx\f$ for any +/// vector \f$x\f$, where \f$K\f$ is positive definite, \f$K_G\f$ is symmetric, +/// and \f$\sigma\f$ is a real shift. +/// This class is intended for internal use. +/// +template , + typename BOpType = SparseSymMatProd> +class SymGEigsBucklingOp +{ +public: + using Scalar = typename OpType::Scalar; + +private: + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + + OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; // temporary working space + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param op The \f$(K-\sigma K_G)^{-1}\f$ matrix operation object. + /// \param Bop The \f$K\f$ matrix operation object. + /// + SymGEigsBucklingOp(OpType& op, const BOpType& Bop) : + m_op(op), m_Bop(Bop), m_cache(op.rows()) + {} + + /// + /// Move constructor. + /// + SymGEigsBucklingOp(SymGEigsBucklingOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_op.rows(); } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_op.rows(); } + + /// + /// Set the real shift \f$\sigma\f$. + /// + void set_shift(const Scalar& sigma) + { + m_op.set_shift(sigma); + } + + /// + /// Perform the matrix operation \f$y=(K-\sigma K_G)^{-1}Kx\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = inv(K - sigma * K_G) * K * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + m_Bop.perform_op(x_in, m_cache.data()); + m_op.perform_op(m_cache.data(), y_out); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_SYM_GEIGS_BUCKLING_OP_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCayleyOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCayleyOp.h new file mode 100644 index 0000000000..df71507136 --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCayleyOp.h @@ -0,0 +1,105 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SYM_GEIGS_CAYLEY_OP_H +#define SPECTRA_SYM_GEIGS_CAYLEY_OP_H + +#include + +#include "../SymShiftInvert.h" +#include "../SparseSymMatProd.h" + +namespace Spectra { + +/// +/// \ingroup Operators +/// +/// This class defines the matrix operation for generalized eigen solver in the +/// Cayley mode. It computes \f$y=(A-\sigma B)^{-1}(A+\sigma B)x\f$ for any +/// vector \f$x\f$, where \f$A\f$ is a symmetric matrix, \f$B\f$ is positive definite, +/// and \f$\sigma\f$ is a real shift. +/// This class is intended for internal use. +/// +template , + typename BOpType = SparseSymMatProd> +class SymGEigsCayleyOp +{ +public: + using Scalar = typename OpType::Scalar; + +private: + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + + OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; // temporary working space + Scalar m_sigma; + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param op The \f$(A-\sigma B)^{-1}\f$ matrix operation object. + /// \param Bop The \f$B\f$ matrix operation object. + /// + SymGEigsCayleyOp(OpType& op, const BOpType& Bop) : + m_op(op), m_Bop(Bop), m_cache(op.rows()) + {} + + /// + /// Move constructor. + /// + SymGEigsCayleyOp(SymGEigsCayleyOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop), m_sigma(other.m_sigma) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_op.rows(); } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_op.rows(); } + + /// + /// Set the real shift \f$\sigma\f$. + /// + void set_shift(const Scalar& sigma) + { + m_op.set_shift(sigma); + m_sigma = sigma; + } + + /// + /// Perform the matrix operation \f$y=(A-\sigma B)^{-1}(A+\sigma B)x\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = inv(A - sigma * B) * (A + sigma * B) * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + // inv(A - sigma * B) * (A + sigma * B) * x + // = inv(A - sigma * B) * (A - sigma * B + 2 * sigma * B) * x + // = x + 2 * sigma * inv(A - sigma * B) * B * x + m_Bop.perform_op(x_in, m_cache.data()); + m_op.perform_op(m_cache.data(), y_out); + MapConstVec x(x_in, this->rows()); + MapVec y(y_out, this->rows()); + y.noalias() = x + (Scalar(2) * m_sigma) * y; + } +}; + +} // namespace Spectra + +#endif // SPECTRA_SYM_GEIGS_CAYLEY_OP_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCholeskyOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCholeskyOp.h index fa99583525..7c1d6bd22d 100644 --- a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCholeskyOp.h +++ b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsCholeskyOp.h @@ -1,13 +1,14 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SYM_GEIGS_CHOLESKY_OP_H -#define SYM_GEIGS_CHOLESKY_OP_H +#ifndef SPECTRA_SYM_GEIGS_CHOLESKY_OP_H +#define SPECTRA_SYM_GEIGS_CHOLESKY_OP_H #include + #include "../DenseSymMatProd.h" #include "../DenseCholesky.h" @@ -21,31 +22,42 @@ namespace Spectra { /// vector \f$x\f$, where \f$L\f$ is the Cholesky decomposition of \f$B\f$. /// This class is intended for internal use. /// -template , - typename BOpType = DenseCholesky > +template , + typename BOpType = DenseCholesky> class SymGEigsCholeskyOp { +public: + using Scalar = typename OpType::Scalar; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; - OpType& m_op; - BOpType& m_Bop; - Vector m_cache; // temporary working space + const OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; // temporary working space public: /// /// Constructor to create the matrix operation object. /// - /// \param op Pointer to the \f$A\f$ matrix operation object. - /// \param Bop Pointer to the \f$B\f$ matrix operation object. + /// \param op The \f$A\f$ matrix operation object. + /// \param Bop The \f$B\f$ matrix operation object. /// - SymGEigsCholeskyOp(OpType& op, BOpType& Bop) : + SymGEigsCholeskyOp(const OpType& op, const BOpType& Bop) : m_op(op), m_Bop(Bop), m_cache(op.rows()) {} + /// + /// Move constructor. + /// + SymGEigsCholeskyOp(SymGEigsCholeskyOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + /// /// Return the number of rows of the underlying matrix. /// @@ -62,7 +74,7 @@ class SymGEigsCholeskyOp /// \param y_out Pointer to the \f$y\f$ vector. /// // y_out = inv(L) * A * inv(L') * x_in - void perform_op(const Scalar* x_in, Scalar* y_out) + void perform_op(const Scalar* x_in, Scalar* y_out) const { m_Bop.upper_triangular_solve(x_in, y_out); m_op.perform_op(y_out, m_cache.data()); @@ -72,4 +84,4 @@ class SymGEigsCholeskyOp } // namespace Spectra -#endif // SYM_GEIGS_CHOLESKY_OP_H +#endif // SPECTRA_SYM_GEIGS_CHOLESKY_OP_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsRegInvOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsRegInvOp.h index 514269c7f6..a8b8482014 100644 --- a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsRegInvOp.h +++ b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsRegInvOp.h @@ -1,13 +1,14 @@ -// Copyright (C) 2017-2019 Yixuan Qiu +// Copyright (C) 2017-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SYM_GEIGS_REG_INV_OP_H -#define SYM_GEIGS_REG_INV_OP_H +#ifndef SPECTRA_SYM_GEIGS_REG_INV_OP_H +#define SPECTRA_SYM_GEIGS_REG_INV_OP_H #include + #include "../SparseSymMatProd.h" #include "../SparseRegularInverse.h" @@ -19,31 +20,42 @@ namespace Spectra { /// This class defines the matrix operation for generalized eigen solver in the /// regular inverse mode. This class is intended for internal use. /// -template , - typename BOpType = SparseRegularInverse > +template , + typename BOpType = SparseRegularInverse> class SymGEigsRegInvOp { +public: + using Scalar = typename OpType::Scalar; + private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; - OpType& m_op; - BOpType& m_Bop; - Vector m_cache; // temporary working space + const OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; // temporary working space public: /// /// Constructor to create the matrix operation object. /// - /// \param op Pointer to the \f$A\f$ matrix operation object. - /// \param Bop Pointer to the \f$B\f$ matrix operation object. + /// \param op The \f$A\f$ matrix operation object. + /// \param Bop The \f$B\f$ matrix operation object. /// - SymGEigsRegInvOp(OpType& op, BOpType& Bop) : + SymGEigsRegInvOp(const OpType& op, const BOpType& Bop) : m_op(op), m_Bop(Bop), m_cache(op.rows()) {} + /// + /// Move constructor. + /// + SymGEigsRegInvOp(SymGEigsRegInvOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + /// /// Return the number of rows of the underlying matrix. /// @@ -60,7 +72,7 @@ class SymGEigsRegInvOp /// \param y_out Pointer to the \f$y\f$ vector. /// // y_out = inv(B) * A * x_in - void perform_op(const Scalar* x_in, Scalar* y_out) + void perform_op(const Scalar* x_in, Scalar* y_out) const { m_op.perform_op(x_in, m_cache.data()); m_Bop.solve(m_cache.data(), y_out); @@ -69,4 +81,4 @@ class SymGEigsRegInvOp } // namespace Spectra -#endif // SYM_GEIGS_REG_INV_OP_H +#endif // SPECTRA_SYM_GEIGS_REG_INV_OP_H diff --git a/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsShiftInvertOp.h b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsShiftInvertOp.h new file mode 100644 index 0000000000..c9e679305f --- /dev/null +++ b/gtsam/3rdparty/Spectra/MatOp/internal/SymGEigsShiftInvertOp.h @@ -0,0 +1,95 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H +#define SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H + +#include + +#include "../SymShiftInvert.h" +#include "../SparseSymMatProd.h" + +namespace Spectra { + +/// +/// \ingroup Operators +/// +/// This class defines the matrix operation for generalized eigen solver in the +/// shift-and-invert mode. It computes \f$y=(A-\sigma B)^{-1}Bx\f$ for any +/// vector \f$x\f$, where \f$A\f$ is a symmetric matrix, \f$B\f$ is positive definite, +/// and \f$\sigma\f$ is a real shift. +/// This class is intended for internal use. +/// +template , + typename BOpType = SparseSymMatProd> +class SymGEigsShiftInvertOp +{ +public: + using Scalar = typename OpType::Scalar; + +private: + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + + OpType& m_op; + const BOpType& m_Bop; + mutable Vector m_cache; // temporary working space + +public: + /// + /// Constructor to create the matrix operation object. + /// + /// \param op The \f$(A-\sigma B)^{-1}\f$ matrix operation object. + /// \param Bop The \f$B\f$ matrix operation object. + /// + SymGEigsShiftInvertOp(OpType& op, const BOpType& Bop) : + m_op(op), m_Bop(Bop), m_cache(op.rows()) + {} + + /// + /// Move constructor. + /// + SymGEigsShiftInvertOp(SymGEigsShiftInvertOp&& other) : + m_op(other.m_op), m_Bop(other.m_Bop) + { + // We emulate the move constructor for Vector using Vector::swap() + m_cache.swap(other.m_cache); + } + + /// + /// Return the number of rows of the underlying matrix. + /// + Index rows() const { return m_op.rows(); } + /// + /// Return the number of columns of the underlying matrix. + /// + Index cols() const { return m_op.rows(); } + + /// + /// Set the real shift \f$\sigma\f$. + /// + void set_shift(const Scalar& sigma) + { + m_op.set_shift(sigma); + } + + /// + /// Perform the matrix operation \f$y=(A-\sigma B)^{-1}Bx\f$. + /// + /// \param x_in Pointer to the \f$x\f$ vector. + /// \param y_out Pointer to the \f$y\f$ vector. + /// + // y_out = inv(A - sigma * B) * B * x_in + void perform_op(const Scalar* x_in, Scalar* y_out) const + { + m_Bop.perform_op(x_in, m_cache.data()); + m_op.perform_op(m_cache.data(), y_out); + } +}; + +} // namespace Spectra + +#endif // SPECTRA_SYM_GEIGS_SHIFT_INVERT_OP_H diff --git a/gtsam/3rdparty/Spectra/SymEigsBase.h b/gtsam/3rdparty/Spectra/SymEigsBase.h deleted file mode 100644 index 9601425d5e..0000000000 --- a/gtsam/3rdparty/Spectra/SymEigsBase.h +++ /dev/null @@ -1,448 +0,0 @@ -// Copyright (C) 2018-2019 Yixuan Qiu -// -// This Source Code Form is subject to the terms of the Mozilla -// Public License v. 2.0. If a copy of the MPL was not distributed -// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. - -#ifndef SYM_EIGS_BASE_H -#define SYM_EIGS_BASE_H - -#include -#include // std::vector -#include // std::abs, std::pow, std::sqrt -#include // std::min, std::copy -#include // std::invalid_argument - -#include "Util/TypeTraits.h" -#include "Util/SelectionRule.h" -#include "Util/CompInfo.h" -#include "Util/SimpleRandom.h" -#include "MatOp/internal/ArnoldiOp.h" -#include "LinAlg/UpperHessenbergQR.h" -#include "LinAlg/TridiagEigen.h" -#include "LinAlg/Lanczos.h" - -namespace Spectra { - -/// -/// \defgroup EigenSolver Eigen Solvers -/// -/// Eigen solvers for different types of problems. -/// - -/// -/// \ingroup EigenSolver -/// -/// This is the base class for symmetric eigen solvers, mainly for internal use. -/// It is kept here to provide the documentation for member functions of concrete eigen solvers -/// such as SymEigsSolver and SymEigsShiftSolver. -/// -template -class SymEigsBase -{ -private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef Eigen::Array Array; - typedef Eigen::Array BoolArray; - typedef Eigen::Map MapMat; - typedef Eigen::Map MapVec; - typedef Eigen::Map MapConstVec; - - typedef ArnoldiOp ArnoldiOpType; - typedef Lanczos LanczosFac; - -protected: - // clang-format off - OpType* m_op; // object to conduct matrix operation, - // e.g. matrix-vector product - const Index m_n; // dimension of matrix A - const Index m_nev; // number of eigenvalues requested - const Index m_ncv; // dimension of Krylov subspace in the Lanczos method - Index m_nmatop; // number of matrix operations called - Index m_niter; // number of restarting iterations - - LanczosFac m_fac; // Lanczos factorization - Vector m_ritz_val; // Ritz values - -private: - Matrix m_ritz_vec; // Ritz vectors - Vector m_ritz_est; // last row of m_ritz_vec, also called the Ritz estimates - BoolArray m_ritz_conv; // indicator of the convergence of Ritz values - int m_info; // status of the computation - - const Scalar m_near_0; // a very small value, but 1.0 / m_near_0 does not overflow - // ~= 1e-307 for the "double" type - const Scalar m_eps; // the machine precision, ~= 1e-16 for the "double" type - const Scalar m_eps23; // m_eps^(2/3), used to test the convergence - // clang-format on - - // Implicitly restarted Lanczos factorization - void restart(Index k) - { - if (k >= m_ncv) - return; - - TridiagQR decomp(m_ncv); - Matrix Q = Matrix::Identity(m_ncv, m_ncv); - - for (Index i = k; i < m_ncv; i++) - { - // QR decomposition of H-mu*I, mu is the shift - decomp.compute(m_fac.matrix_H(), m_ritz_val[i]); - - // Q -> Q * Qi - decomp.apply_YQ(Q); - // H -> Q'HQ - // Since QR = H - mu * I, we have H = QR + mu * I - // and therefore Q'HQ = RQ + mu * I - m_fac.compress_H(decomp); - } - - m_fac.compress_V(Q); - m_fac.factorize_from(k, m_ncv, m_nmatop); - - retrieve_ritzpair(); - } - - // Calculates the number of converged Ritz values - Index num_converged(Scalar tol) - { - // thresh = tol * max(m_eps23, abs(theta)), theta for Ritz value - Array thresh = tol * m_ritz_val.head(m_nev).array().abs().max(m_eps23); - Array resid = m_ritz_est.head(m_nev).array().abs() * m_fac.f_norm(); - // Converged "wanted" Ritz values - m_ritz_conv = (resid < thresh); - - return m_ritz_conv.cast().sum(); - } - - // Returns the adjusted nev for restarting - Index nev_adjusted(Index nconv) - { - using std::abs; - - Index nev_new = m_nev; - for (Index i = m_nev; i < m_ncv; i++) - if (abs(m_ritz_est[i]) < m_near_0) - nev_new++; - - // Adjust nev_new, according to dsaup2.f line 677~684 in ARPACK - nev_new += std::min(nconv, (m_ncv - nev_new) / 2); - if (nev_new == 1 && m_ncv >= 6) - nev_new = m_ncv / 2; - else if (nev_new == 1 && m_ncv > 2) - nev_new = 2; - - if (nev_new > m_ncv - 1) - nev_new = m_ncv - 1; - - return nev_new; - } - - // Retrieves and sorts Ritz values and Ritz vectors - void retrieve_ritzpair() - { - TridiagEigen decomp(m_fac.matrix_H()); - const Vector& evals = decomp.eigenvalues(); - const Matrix& evecs = decomp.eigenvectors(); - - SortEigenvalue sorting(evals.data(), evals.size()); - std::vector ind = sorting.index(); - - // For BOTH_ENDS, the eigenvalues are sorted according - // to the LARGEST_ALGE rule, so we need to move those smallest - // values to the left - // The order would be - // Largest => Smallest => 2nd largest => 2nd smallest => ... - // We keep this order since the first k values will always be - // the wanted collection, no matter k is nev_updated (used in restart()) - // or is nev (used in sort_ritzpair()) - if (SelectionRule == BOTH_ENDS) - { - std::vector ind_copy(ind); - for (Index i = 0; i < m_ncv; i++) - { - // If i is even, pick values from the left (large values) - // If i is odd, pick values from the right (small values) - if (i % 2 == 0) - ind[i] = ind_copy[i / 2]; - else - ind[i] = ind_copy[m_ncv - 1 - i / 2]; - } - } - - // Copy the Ritz values and vectors to m_ritz_val and m_ritz_vec, respectively - for (Index i = 0; i < m_ncv; i++) - { - m_ritz_val[i] = evals[ind[i]]; - m_ritz_est[i] = evecs(m_ncv - 1, ind[i]); - } - for (Index i = 0; i < m_nev; i++) - { - m_ritz_vec.col(i).noalias() = evecs.col(ind[i]); - } - } - -protected: - // Sorts the first nev Ritz pairs in the specified order - // This is used to return the final results - virtual void sort_ritzpair(int sort_rule) - { - // First make sure that we have a valid index vector - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - std::vector ind = sorting.index(); - - switch (sort_rule) - { - case LARGEST_ALGE: - break; - case LARGEST_MAGN: - { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); - break; - } - case SMALLEST_ALGE: - { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); - break; - } - case SMALLEST_MAGN: - { - SortEigenvalue sorting(m_ritz_val.data(), m_nev); - ind = sorting.index(); - break; - } - default: - throw std::invalid_argument("unsupported sorting rule"); - } - - Vector new_ritz_val(m_ncv); - Matrix new_ritz_vec(m_ncv, m_nev); - BoolArray new_ritz_conv(m_nev); - - for (Index i = 0; i < m_nev; i++) - { - new_ritz_val[i] = m_ritz_val[ind[i]]; - new_ritz_vec.col(i).noalias() = m_ritz_vec.col(ind[i]); - new_ritz_conv[i] = m_ritz_conv[ind[i]]; - } - - m_ritz_val.swap(new_ritz_val); - m_ritz_vec.swap(new_ritz_vec); - m_ritz_conv.swap(new_ritz_conv); - } - -public: - /// \cond - - SymEigsBase(OpType* op, BOpType* Bop, Index nev, Index ncv) : - m_op(op), - m_n(m_op->rows()), - m_nev(nev), - m_ncv(ncv > m_n ? m_n : ncv), - m_nmatop(0), - m_niter(0), - m_fac(ArnoldiOpType(op, Bop), m_ncv), - m_info(NOT_COMPUTED), - m_near_0(TypeTraits::min() * Scalar(10)), - m_eps(Eigen::NumTraits::epsilon()), - m_eps23(Eigen::numext::pow(m_eps, Scalar(2.0) / 3)) - { - if (nev < 1 || nev > m_n - 1) - throw std::invalid_argument("nev must satisfy 1 <= nev <= n - 1, n is the size of matrix"); - - if (ncv <= nev || ncv > m_n) - throw std::invalid_argument("ncv must satisfy nev < ncv <= n, n is the size of matrix"); - } - - /// - /// Virtual destructor - /// - virtual ~SymEigsBase() {} - - /// \endcond - - /// - /// Initializes the solver by providing an initial residual vector. - /// - /// \param init_resid Pointer to the initial residual vector. - /// - /// **Spectra** (and also **ARPACK**) uses an iterative algorithm - /// to find eigenvalues. This function allows the user to provide the initial - /// residual vector. - /// - void init(const Scalar* init_resid) - { - // Reset all matrices/vectors to zero - m_ritz_val.resize(m_ncv); - m_ritz_vec.resize(m_ncv, m_nev); - m_ritz_est.resize(m_ncv); - m_ritz_conv.resize(m_nev); - - m_ritz_val.setZero(); - m_ritz_vec.setZero(); - m_ritz_est.setZero(); - m_ritz_conv.setZero(); - - m_nmatop = 0; - m_niter = 0; - - // Initialize the Lanczos factorization - MapConstVec v0(init_resid, m_n); - m_fac.init(v0, m_nmatop); - } - - /// - /// Initializes the solver by providing a random initial residual vector. - /// - /// This overloaded function generates a random initial residual vector - /// (with a fixed random seed) for the algorithm. Elements in the vector - /// follow independent Uniform(-0.5, 0.5) distribution. - /// - void init() - { - SimpleRandom rng(0); - Vector init_resid = rng.random_vec(m_n); - init(init_resid.data()); - } - - /// - /// Conducts the major computation procedure. - /// - /// \param maxit Maximum number of iterations allowed in the algorithm. - /// \param tol Precision parameter for the calculated eigenvalues. - /// \param sort_rule Rule to sort the eigenvalues and eigenvectors. - /// Supported values are - /// `Spectra::LARGEST_ALGE`, `Spectra::LARGEST_MAGN`, - /// `Spectra::SMALLEST_ALGE` and `Spectra::SMALLEST_MAGN`, - /// for example `LARGEST_ALGE` indicates that largest eigenvalues - /// come first. Note that this argument is only used to - /// **sort** the final result, and the **selection** rule - /// (e.g. selecting the largest or smallest eigenvalues in the - /// full spectrum) is specified by the template parameter - /// `SelectionRule` of SymEigsSolver. - /// - /// \return Number of converged eigenvalues. - /// - Index compute(Index maxit = 1000, Scalar tol = 1e-10, int sort_rule = LARGEST_ALGE) - { - // The m-step Lanczos factorization - m_fac.factorize_from(1, m_ncv, m_nmatop); - retrieve_ritzpair(); - // Restarting - Index i, nconv = 0, nev_adj; - for (i = 0; i < maxit; i++) - { - 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; - - return std::min(m_nev, nconv); - } - - /// - /// Returns the status of the computation. - /// The full list of enumeration values can be found in \ref Enumerations. - /// - int info() const { return m_info; } - - /// - /// Returns the number of iterations used in the computation. - /// - Index num_iterations() const { return m_niter; } - - /// - /// Returns the number of matrix operations used in the computation. - /// - Index num_operations() const { return m_nmatop; } - - /// - /// Returns the converged eigenvalues. - /// - /// \return A vector containing the eigenvalues. - /// Returned vector type will be `Eigen::Vector`, depending on - /// the template parameter `Scalar` defined. - /// - Vector eigenvalues() const - { - const Index nconv = m_ritz_conv.cast().sum(); - Vector res(nconv); - - if (!nconv) - return res; - - Index j = 0; - for (Index i = 0; i < m_nev; i++) - { - if (m_ritz_conv[i]) - { - res[j] = m_ritz_val[i]; - j++; - } - } - - return res; - } - - /// - /// Returns the eigenvectors associated with the converged eigenvalues. - /// - /// \param nvec The number of eigenvectors to return. - /// - /// \return A matrix containing the eigenvectors. - /// Returned matrix type will be `Eigen::Matrix`, - /// depending on the template parameter `Scalar` defined. - /// - virtual Matrix eigenvectors(Index nvec) const - { - const Index nconv = m_ritz_conv.cast().sum(); - nvec = std::min(nvec, nconv); - Matrix res(m_n, nvec); - - if (!nvec) - return res; - - Matrix ritz_vec_conv(m_ncv, nvec); - Index j = 0; - for (Index i = 0; i < m_nev && j < nvec; i++) - { - if (m_ritz_conv[i]) - { - ritz_vec_conv.col(j).noalias() = m_ritz_vec.col(i); - j++; - } - } - - res.noalias() = m_fac.matrix_V() * ritz_vec_conv; - - return res; - } - - /// - /// Returns all converged eigenvectors. - /// - virtual Matrix eigenvectors() const - { - return eigenvectors(m_nev); - } -}; - -} // namespace Spectra - -#endif // SYM_EIGS_BASE_H diff --git a/gtsam/3rdparty/Spectra/SymEigsShiftSolver.h b/gtsam/3rdparty/Spectra/SymEigsShiftSolver.h index e2b410cb11..373360143d 100644 --- a/gtsam/3rdparty/Spectra/SymEigsShiftSolver.h +++ b/gtsam/3rdparty/Spectra/SymEigsShiftSolver.h @@ -1,15 +1,15 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SYM_EIGS_SHIFT_SOLVER_H -#define SYM_EIGS_SHIFT_SOLVER_H +#ifndef SPECTRA_SYM_EIGS_SHIFT_SOLVER_H +#define SPECTRA_SYM_EIGS_SHIFT_SOLVER_H #include -#include "SymEigsBase.h" +#include "HermEigsBase.h" #include "Util/SelectionRule.h" #include "MatOp/DenseSymShiftSolve.h" @@ -54,17 +54,11 @@ namespace Spectra { /// returning \f$\lambda\f$ rather than \f$\nu\f$), and eigenvectors are the /// same for both the original problem and the shifted-and-inverted problem. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the shifted-and-inverted eigenvalues. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class. Users could either -/// use the wrapper classes such as DenseSymShiftSolve and -/// SparseSymShiftSolve, or define their -/// own that implements all the public member functions as in -/// DenseSymShiftSolve. +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseSymShiftSolve and +/// SparseSymShiftSolve, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseSymShiftSolve. /// /// Below is an example that illustrates the use of the shift-and-invert mode: /// @@ -80,7 +74,7 @@ namespace Spectra { /// { /// // A size-10 diagonal matrix with elements 1, 2, ..., 10 /// Eigen::MatrixXd M = Eigen::MatrixXd::Zero(10, 10); -/// for(int i = 0; i < M.rows(); i++) +/// for (int i = 0; i < M.rows(); i++) /// M(i, i) = i + 1; /// /// // Construct matrix operation object using the wrapper class @@ -88,12 +82,11 @@ namespace Spectra { /// /// // Construct eigen solver object with shift 0 /// // This will find eigenvalues that are closest to 0 -/// SymEigsShiftSolver< double, LARGEST_MAGN, -/// DenseSymShiftSolve > eigs(&op, 3, 6, 0.0); +/// SymEigsShiftSolver> eigs(op, 3, 6, 0.0); /// /// eigs.init(); -/// eigs.compute(); -/// if(eigs.info() == SUCCESSFUL) +/// eigs.compute(SortRule::LargestMagn); +/// if (eigs.info() == CompInfo::Successful) /// { /// Eigen::VectorXd evalues = eigs.eigenvalues(); /// // Will get (3.0, 2.0, 1.0) @@ -119,14 +112,15 @@ namespace Spectra { /// private: /// double sigma_; /// public: -/// int rows() { return 10; } -/// int cols() { return 10; } +/// using Scalar = double; // A typedef named "Scalar" is required +/// int rows() const { return 10; } +/// int cols() const { return 10; } /// void set_shift(double sigma) { sigma_ = sigma; } /// // y_out = inv(A - sigma * I) * x_in /// // inv(A - sigma * I) = diag(1/(1-sigma), 1/(2-sigma), ...) -/// void perform_op(double *x_in, double *y_out) +/// void perform_op(double *x_in, double *y_out) const /// { -/// for(int i = 0; i < rows(); i++) +/// for (int i = 0; i < rows(); i++) /// { /// y_out[i] = x_in[i] / (i + 1 - sigma_); /// } @@ -137,11 +131,10 @@ namespace Spectra { /// { /// MyDiagonalTenShiftSolve op; /// // Find three eigenvalues that are closest to 3.14 -/// SymEigsShiftSolver eigs(&op, 3, 6, 3.14); +/// SymEigsShiftSolver eigs(op, 3, 6, 3.14); /// eigs.init(); -/// eigs.compute(); -/// if(eigs.info() == SUCCESSFUL) +/// eigs.compute(SortRule::LargestMagn); +/// if (eigs.info() == CompInfo::Successful) /// { /// Eigen::VectorXd evalues = eigs.eigenvalues(); /// // Will get (4.0, 3.0, 2.0) @@ -152,34 +145,38 @@ namespace Spectra { /// } /// \endcode /// -template > -class SymEigsShiftSolver : public SymEigsBase +template > +class SymEigsShiftSolver : public HermEigsBase { private: - typedef Eigen::Index Index; - typedef Eigen::Array Array; + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Array = Eigen::Array; + + using Base = HermEigsBase; + using Base::m_nev; + using Base::m_ritz_val; const Scalar m_sigma; // First transform back the Ritz values, and then sort - void sort_ritzpair(int sort_rule) + void sort_ritzpair(SortRule sort_rule) override { - Array m_ritz_val_org = Scalar(1.0) / this->m_ritz_val.head(this->m_nev).array() + m_sigma; - this->m_ritz_val.head(this->m_nev) = m_ritz_val_org; - SymEigsBase::sort_ritzpair(sort_rule); + // The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma) + // So the eigenvalues of the original problem is lambda = 1 / nu + sigma + m_ritz_val.head(m_nev).array() = Scalar(1) / m_ritz_val.head(m_nev).array() + m_sigma; + Base::sort_ritzpair(sort_rule); } public: /// /// Constructor to create a eigen solver object using the shift-and-invert mode. /// - /// \param op Pointer to the matrix operation object, which should implement + /// \param op The matrix operation object that implements /// the shift-solve operation of \f$A\f$: calculating /// \f$(A-\sigma I)^{-1}v\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper class such as DenseSymShiftSolve, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseSymShiftSolve. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, /// where \f$n\f$ is the size of matrix. @@ -190,14 +187,14 @@ class SymEigsShiftSolver : public SymEigsBase(op, NULL, nev, ncv), + SymEigsShiftSolver(OpType& op, Index nev, Index ncv, const Scalar& sigma) : + Base(op, IdentityBOp(), nev, ncv), m_sigma(sigma) { - this->m_op->set_shift(m_sigma); + op.set_shift(m_sigma); } }; } // namespace Spectra -#endif // SYM_EIGS_SHIFT_SOLVER_H +#endif // SPECTRA_SYM_EIGS_SHIFT_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/SymEigsSolver.h b/gtsam/3rdparty/Spectra/SymEigsSolver.h index 404df1e522..a19956a130 100644 --- a/gtsam/3rdparty/Spectra/SymEigsSolver.h +++ b/gtsam/3rdparty/Spectra/SymEigsSolver.h @@ -1,15 +1,15 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SYM_EIGS_SOLVER_H -#define SYM_EIGS_SOLVER_H +#ifndef SPECTRA_SYM_EIGS_SOLVER_H +#define SPECTRA_SYM_EIGS_SOLVER_H #include -#include "SymEigsBase.h" +#include "HermEigsBase.h" #include "Util/SelectionRule.h" #include "MatOp/DenseSymMatProd.h" @@ -34,27 +34,21 @@ namespace Spectra { /// the constructor of SymEigsSolver. /// /// If the matrix \f$A\f$ is already stored as a matrix object in **Eigen**, -/// for example `Eigen::MatrixXd`, then there is an easy way to construct such +/// for example `Eigen::MatrixXd`, then there is an easy way to construct such a /// matrix operation class, by using the built-in wrapper class DenseSymMatProd -/// which wraps an existing matrix object in **Eigen**. This is also the +/// that wraps an existing matrix object in **Eigen**. This is also the /// default template parameter for SymEigsSolver. For sparse matrices, the /// wrapper class SparseSymMatProd can be used similarly. /// /// If the users need to define their own matrix-vector multiplication operation -/// class, it should implement all the public member functions as in DenseSymMatProd. -/// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the requested eigenvalues, for example `LARGEST_MAGN` -/// to retrieve eigenvalues with the largest magnitude. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class. Users could either -/// use the wrapper classes such as DenseSymMatProd and -/// SparseSymMatProd, or define their -/// own that implements all the public member functions as in -/// DenseSymMatProd. +/// class, it should define a public type `Scalar` to indicate the element type, +/// and implement all the public member functions as in DenseSymMatProd. +/// +/// \tparam OpType The name of the matrix operation class. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseSymMatProd. /// /// Below is an example that demonstrates the usage of this class. /// @@ -76,15 +70,15 @@ namespace Spectra { /// DenseSymMatProd op(M); /// /// // Construct eigen solver object, requesting the largest three eigenvalues -/// SymEigsSolver< double, LARGEST_ALGE, DenseSymMatProd > eigs(&op, 3, 6); +/// SymEigsSolver> eigs(op, 3, 6); /// /// // Initialize and compute /// eigs.init(); -/// int nconv = eigs.compute(); +/// int nconv = eigs.compute(SortRule::LargestAlge); /// /// // Retrieve results /// Eigen::VectorXd evalues; -/// if(eigs.info() == SUCCESSFUL) +/// if (eigs.info() == CompInfo::Successful) /// evalues = eigs.eigenvalues(); /// /// std::cout << "Eigenvalues found:\n" << evalues << std::endl; @@ -106,12 +100,13 @@ namespace Spectra { /// class MyDiagonalTen /// { /// public: -/// int rows() { return 10; } -/// int cols() { return 10; } +/// using Scalar = double; // A typedef named "Scalar" is required +/// int rows() const { return 10; } +/// int cols() const { return 10; } /// // y_out = M * x_in -/// void perform_op(double *x_in, double *y_out) +/// void perform_op(double *x_in, double *y_out) const /// { -/// for(int i = 0; i < rows(); i++) +/// for (int i = 0; i < rows(); i++) /// { /// y_out[i] = x_in[i] * (i + 1); /// } @@ -121,10 +116,10 @@ namespace Spectra { /// int main() /// { /// MyDiagonalTen op; -/// SymEigsSolver eigs(&op, 3, 6); +/// SymEigsSolver eigs(op, 3, 6); /// eigs.init(); -/// eigs.compute(); -/// if(eigs.info() == SUCCESSFUL) +/// eigs.compute(SortRule::LargestAlge); +/// if (eigs.info() == CompInfo::Successful) /// { /// Eigen::VectorXd evalues = eigs.eigenvalues(); /// // Will get (10, 9, 8) @@ -135,23 +130,21 @@ namespace Spectra { /// } /// \endcode /// -template > -class SymEigsSolver : public SymEigsBase +template > +class SymEigsSolver : public HermEigsBase { private: - typedef Eigen::Index Index; + using Index = Eigen::Index; public: /// /// Constructor to create a solver object. /// - /// \param op Pointer to the matrix operation object, which should implement + /// \param op The matrix operation object that implements /// the matrix-vector multiplication operation of \f$A\f$: /// calculating \f$Av\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper class such as DenseSymMatProd, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseSymMatProd. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, /// where \f$n\f$ is the size of matrix. @@ -161,11 +154,11 @@ class SymEigsSolver : public SymEigsBase(op, NULL, nev, ncv) + SymEigsSolver(OpType& op, Index nev, Index ncv) : + HermEigsBase(op, IdentityBOp(), nev, ncv) {} }; } // namespace Spectra -#endif // SYM_EIGS_SOLVER_H +#endif // SPECTRA_SYM_EIGS_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/SymGEigsShiftSolver.h b/gtsam/3rdparty/Spectra/SymGEigsShiftSolver.h new file mode 100644 index 0000000000..cca5b7ec73 --- /dev/null +++ b/gtsam/3rdparty/Spectra/SymGEigsShiftSolver.h @@ -0,0 +1,463 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_SYM_GEIGS_SHIFT_SOLVER_H +#define SPECTRA_SYM_GEIGS_SHIFT_SOLVER_H + +#include // std::move +#include "HermEigsBase.h" +#include "Util/GEigsMode.h" +#include "MatOp/internal/SymGEigsShiftInvertOp.h" +#include "MatOp/internal/SymGEigsBucklingOp.h" +#include "MatOp/internal/SymGEigsCayleyOp.h" + +namespace Spectra { + +/// +/// \ingroup GEigenSolver +/// +/// This class implements the generalized eigen solver for real symmetric +/// matrices, i.e., to solve \f$Ax=\lambda Bx\f$ where \f$A\f$ and \f$B\f$ are symmetric +/// matrices. A spectral transform is applied to seek interior +/// generalized eigenvalues with respect to some shift \f$\sigma\f$. +/// +/// There are different modes of this solver, specified by the template parameter `Mode`. +/// See the pages for the specialized classes for details. +/// - The shift-and-invert mode transforms the problem into \f$(A-\sigma B)^{-1}Bx=\nu x\f$, +/// where \f$\nu=1/(\lambda-\sigma)\f$. This mode assumes that \f$B\f$ is positive definite. +/// See \ref SymGEigsShiftSolver +/// "SymGEigsShiftSolver (Shift-and-invert mode)" for more details. +/// - The buckling mode transforms the problem into \f$(A-\sigma B)^{-1}Ax=\nu x\f$, +/// where \f$\nu=\lambda/(\lambda-\sigma)\f$. This mode assumes that \f$A\f$ is positive definite. +/// See \ref SymGEigsShiftSolver +/// "SymGEigsShiftSolver (Buckling mode)" for more details. +/// - The Cayley mode transforms the problem into \f$(A-\sigma B)^{-1}(A+\sigma B)x=\nu x\f$, +/// where \f$\nu=(\lambda+\sigma)/(\lambda-\sigma)\f$. This mode assumes that \f$B\f$ is positive definite. +/// See \ref SymGEigsShiftSolver +/// "SymGEigsShiftSolver (Cayley mode)" for more details. + +// Empty class template +template +class SymGEigsShiftSolver +{}; + +/// +/// \ingroup GEigenSolver +/// +/// This class implements the generalized eigen solver for real symmetric +/// matrices using the shift-and-invert spectral transformation. The original problem is +/// to solve \f$Ax=\lambda Bx\f$, where \f$A\f$ is symmetric and \f$B\f$ is positive definite. +/// The transformed problem is \f$(A-\sigma B)^{-1}Bx=\nu x\f$, where +/// \f$\nu=1/(\lambda-\sigma)\f$, and \f$\sigma\f$ is a user-specified shift. +/// +/// This solver requires two matrix operation objects: one to compute \f$y=(A-\sigma B)^{-1}x\f$ +/// for any vector \f$v\f$, and one for the matrix multiplication \f$Bv\f$. +/// +/// If \f$A\f$ and \f$B\f$ are stored as Eigen matrices, then the first operation object +/// can be created using the SymShiftInvert class, and the second one can be created +/// using the DenseSymMatProd or SparseSymMatProd classes. If the users need to define their +/// own operation classes, then they should implement all the public member functions as +/// in those built-in classes. +/// +/// \tparam OpType The type of the first operation object. Users could either +/// use the wrapper class SymShiftInvert, or define their own that implements +/// the type definition `Scalar` and all the public member functions as in SymShiftInvert. +/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements all the +/// public member functions as in DenseSymMatProd. +/// \tparam Mode Mode of the generalized eigen solver. In this solver +/// it is Spectra::GEigsMode::ShiftInvert. +/// +/// Below is an example that demonstrates the usage of this class. +/// +/// \code{.cpp} +/// #include +/// #include +/// #include +/// #include +/// #include +/// #include +/// +/// using namespace Spectra; +/// +/// int main() +/// { +/// // We are going to solve the generalized eigenvalue problem +/// // A * x = lambda * B * x, +/// // where A is symmetric and B is positive definite +/// const int n = 100; +/// +/// // Define the A matrix +/// Eigen::MatrixXd M = Eigen::MatrixXd::Random(n, n); +/// Eigen::MatrixXd A = M + M.transpose(); +/// +/// // Define the B matrix, a tridiagonal matrix with 2 on the diagonal +/// // and 1 on the subdiagonals +/// Eigen::SparseMatrix B(n, n); +/// B.reserve(Eigen::VectorXi::Constant(n, 3)); +/// for (int i = 0; i < n; i++) +/// { +/// B.insert(i, i) = 2.0; +/// if (i > 0) +/// B.insert(i - 1, i) = 1.0; +/// if (i < n - 1) +/// B.insert(i + 1, i) = 1.0; +/// } +/// +/// // Construct matrix operation objects using the wrapper classes +/// // A is dense, B is sparse +/// using OpType = SymShiftInvert; +/// using BOpType = SparseSymMatProd; +/// OpType op(A, B); +/// BOpType Bop(B); +/// +/// // Construct generalized eigen solver object, seeking three generalized +/// // eigenvalues that are closest to zero. This is equivalent to specifying +/// // a shift sigma = 0.0 combined with the SortRule::LargestMagn selection rule +/// SymGEigsShiftSolver +/// geigs(op, Bop, 3, 6, 0.0); +/// +/// // Initialize and compute +/// geigs.init(); +/// int nconv = geigs.compute(SortRule::LargestMagn); +/// +/// // Retrieve results +/// Eigen::VectorXd evalues; +/// Eigen::MatrixXd evecs; +/// if (geigs.info() == CompInfo::Successful) +/// { +/// evalues = geigs.eigenvalues(); +/// evecs = geigs.eigenvectors(); +/// } +/// +/// std::cout << "Number of converged generalized eigenvalues: " << nconv << std::endl; +/// std::cout << "Generalized eigenvalues found:\n" << evalues << std::endl; +/// std::cout << "Generalized eigenvectors found:\n" << evecs.topRows(10) << std::endl; +/// +/// return 0; +/// } +/// \endcode + +// Partial specialization for mode = GEigsMode::ShiftInvert +template +class SymGEigsShiftSolver : + public HermEigsBase, BOpType> +{ +private: + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Array = Eigen::Array; + + using ModeMatOp = SymGEigsShiftInvertOp; + using Base = HermEigsBase; + using Base::m_nev; + using Base::m_ritz_val; + + const Scalar m_sigma; + + // Set shift and forward + static ModeMatOp set_shift_and_move(ModeMatOp&& op, const Scalar& sigma) + { + op.set_shift(sigma); + return std::move(op); + } + + // First transform back the Ritz values, and then sort + void sort_ritzpair(SortRule sort_rule) override + { + // The eigenvalues we get from the iteration is nu = 1 / (lambda - sigma) + // So the eigenvalues of the original problem is lambda = 1 / nu + sigma + m_ritz_val.head(m_nev).array() = Scalar(1) / m_ritz_val.head(m_nev).array() + m_sigma; + Base::sort_ritzpair(sort_rule); + } + +public: + /// + /// Constructor to create a solver object. + /// + /// \param op The matrix operation object that computes \f$y=(A-\sigma B)^{-1}v\f$ + /// for any vector \f$v\f$. Users could either create the object from the + /// wrapper class SymShiftInvert, or define their own that implements all + /// the public members as in SymShiftInvert. + /// \param Bop The \f$B\f$ matrix operation object that implements the matrix-vector + /// multiplication \f$Bv\f$. Users could either create the object from the + /// wrapper classes such as DenseSymMatProd and SparseSymMatProd, or + /// define their own that implements all the public member functions + /// as in DenseSymMatProd. \f$B\f$ needs to be positive definite. + /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, + /// where \f$n\f$ is the size of matrix. + /// \param ncv Parameter that controls the convergence speed of the algorithm. + /// Typically a larger `ncv` means faster convergence, but it may + /// also result in greater memory use and more matrix operations + /// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$, + /// and is advised to take \f$ncv \ge 2\cdot nev\f$. + /// \param sigma The value of the shift. + /// + SymGEigsShiftSolver(OpType& op, BOpType& Bop, Index nev, Index ncv, const Scalar& sigma) : + Base(set_shift_and_move(ModeMatOp(op, Bop), sigma), Bop, nev, ncv), + m_sigma(sigma) + {} +}; + +/// +/// \ingroup GEigenSolver +/// +/// This class implements the generalized eigen solver for real symmetric +/// matrices in the buckling mode. The original problem is +/// to solve \f$Kx=\lambda K_G x\f$, where \f$K\f$ is positive definite and \f$K_G\f$ is symmetric. +/// The transformed problem is \f$(K-\sigma K_G)^{-1}Kx=\nu x\f$, where +/// \f$\nu=\lambda/(\lambda-\sigma)\f$, and \f$\sigma\f$ is a user-specified shift. +/// +/// This solver requires two matrix operation objects: one to compute \f$y=(K-\sigma K_G)^{-1}x\f$ +/// for any vector \f$v\f$, and one for the matrix multiplication \f$Kv\f$. +/// +/// If \f$K\f$ and \f$K_G\f$ are stored as Eigen matrices, then the first operation object +/// can be created using the SymShiftInvert class, and the second one can be created +/// using the DenseSymMatProd or SparseSymMatProd classes. If the users need to define their +/// own operation classes, then they should implement all the public member functions as +/// in those built-in classes. +/// +/// \tparam OpType The type of the first operation object. Users could either +/// use the wrapper class SymShiftInvert, or define their own that implements +/// the type definition `Scalar` and all the public member functions as in SymShiftInvert. +/// \tparam BOpType The name of the matrix operation class for \f$K\f$. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements all the +/// public member functions as in DenseSymMatProd. +/// \tparam Mode Mode of the generalized eigen solver. In this solver +/// it is Spectra::GEigsMode::Buckling. +/// +/// Below is an example that demonstrates the usage of this class. +/// +/// \code{.cpp} +/// #include +/// #include +/// #include +/// #include +/// #include +/// #include +/// +/// using namespace Spectra; +/// +/// int main() +/// { +/// // We are going to solve the generalized eigenvalue problem +/// // K * x = lambda * KG * x, +/// // where K is positive definite, and KG is symmetric +/// const int n = 100; +/// +/// // Define the K matrix, a tridiagonal matrix with 2 on the diagonal +/// // and 1 on the subdiagonals +/// Eigen::SparseMatrix K(n, n); +/// K.reserve(Eigen::VectorXi::Constant(n, 3)); +/// for (int i = 0; i < n; i++) +/// { +/// K.insert(i, i) = 2.0; +/// if (i > 0) +/// K.insert(i - 1, i) = 1.0; +/// if (i < n - 1) +/// K.insert(i + 1, i) = 1.0; +/// } +/// +/// // Define the KG matrix +/// Eigen::MatrixXd M = Eigen::MatrixXd::Random(n, n); +/// Eigen::MatrixXd KG = M + M.transpose(); +/// +/// // Construct matrix operation objects using the wrapper classes +/// // K is sparse, KG is dense +/// using OpType = SymShiftInvert; +/// using BOpType = SparseSymMatProd; +/// OpType op(K, KG); +/// BOpType Bop(K); +/// +/// // Construct generalized eigen solver object, seeking three generalized +/// // eigenvalues that are closest to and larger than 1.0. This is equivalent to +/// // specifying a shift sigma = 1.0 combined with the SortRule::LargestAlge +/// // selection rule +/// SymGEigsShiftSolver +/// geigs(op, Bop, 3, 6, 1.0); +/// +/// // Initialize and compute +/// geigs.init(); +/// int nconv = geigs.compute(SortRule::LargestAlge); +/// +/// // Retrieve results +/// Eigen::VectorXd evalues; +/// Eigen::MatrixXd evecs; +/// if (geigs.info() == CompInfo::Successful) +/// { +/// evalues = geigs.eigenvalues(); +/// evecs = geigs.eigenvectors(); +/// } +/// +/// std::cout << "Number of converged generalized eigenvalues: " << nconv << std::endl; +/// std::cout << "Generalized eigenvalues found:\n" << evalues << std::endl; +/// std::cout << "Generalized eigenvectors found:\n" << evecs.topRows(10) << std::endl; +/// +/// return 0; +/// } +/// \endcode + +// Partial specialization for mode = GEigsMode::Buckling +template +class SymGEigsShiftSolver : + public HermEigsBase, BOpType> +{ +private: + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Array = Eigen::Array; + + using ModeMatOp = SymGEigsBucklingOp; + using Base = HermEigsBase; + using Base::m_nev; + using Base::m_ritz_val; + + const Scalar m_sigma; + + // Set shift and forward + static ModeMatOp set_shift_and_move(ModeMatOp&& op, const Scalar& sigma) + { + if (sigma == Scalar(0)) + throw std::invalid_argument("SymGEigsShiftSolver: sigma cannot be zero in the buckling mode"); + op.set_shift(sigma); + return std::move(op); + } + + // First transform back the Ritz values, and then sort + void sort_ritzpair(SortRule sort_rule) override + { + // The eigenvalues we get from the iteration is nu = lambda / (lambda - sigma) + // So the eigenvalues of the original problem is lambda = sigma * nu / (nu - 1) + m_ritz_val.head(m_nev).array() = m_sigma * m_ritz_val.head(m_nev).array() / + (m_ritz_val.head(m_nev).array() - Scalar(1)); + Base::sort_ritzpair(sort_rule); + } + +public: + /// + /// Constructor to create a solver object. + /// + /// \param op The matrix operation object that computes \f$y=(K-\sigma K_G)^{-1}v\f$ + /// for any vector \f$v\f$. Users could either create the object from the + /// wrapper class SymShiftInvert, or define their own that implements all + /// the public members as in SymShiftInvert. + /// \param Bop The \f$K\f$ matrix operation object that implements the matrix-vector + /// multiplication \f$Kv\f$. Users could either create the object from the + /// wrapper classes such as DenseSymMatProd and SparseSymMatProd, or + /// define their own that implements all the public member functions + /// as in DenseSymMatProd. \f$K\f$ needs to be positive definite. + /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, + /// where \f$n\f$ is the size of matrix. + /// \param ncv Parameter that controls the convergence speed of the algorithm. + /// Typically a larger `ncv` means faster convergence, but it may + /// also result in greater memory use and more matrix operations + /// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$, + /// and is advised to take \f$ncv \ge 2\cdot nev\f$. + /// \param sigma The value of the shift. + /// + SymGEigsShiftSolver(OpType& op, BOpType& Bop, Index nev, Index ncv, const Scalar& sigma) : + Base(set_shift_and_move(ModeMatOp(op, Bop), sigma), Bop, nev, ncv), + m_sigma(sigma) + {} +}; + +/// +/// \ingroup GEigenSolver +/// +/// This class implements the generalized eigen solver for real symmetric +/// matrices using the Cayley spectral transformation. The original problem is +/// to solve \f$Ax=\lambda Bx\f$, where \f$A\f$ is symmetric and \f$B\f$ is positive definite. +/// The transformed problem is \f$(A-\sigma B)^{-1}(A+\sigma B)x=\nu x\f$, where +/// \f$\nu=(\lambda+\sigma)/(\lambda-\sigma)\f$, and \f$\sigma\f$ is a user-specified shift. +/// +/// This solver requires two matrix operation objects: one to compute \f$y=(A-\sigma B)^{-1}x\f$ +/// for any vector \f$v\f$, and one for the matrix multiplication \f$Bv\f$. +/// +/// If \f$A\f$ and \f$B\f$ are stored as Eigen matrices, then the first operation object +/// can be created using the SymShiftInvert class, and the second one can be created +/// using the DenseSymMatProd or SparseSymMatProd classes. If the users need to define their +/// own operation classes, then they should implement all the public member functions as +/// in those built-in classes. +/// +/// \tparam OpType The type of the first operation object. Users could either +/// use the wrapper class SymShiftInvert, or define their own that implements +/// the type definition `Scalar` and all the public member functions as in SymShiftInvert. +/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements all the +/// public member functions as in DenseSymMatProd. +/// \tparam Mode Mode of the generalized eigen solver. In this solver +/// it is Spectra::GEigsMode::Cayley. + +// Partial specialization for mode = GEigsMode::Cayley +template +class SymGEigsShiftSolver : + public HermEigsBase, BOpType> +{ +private: + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Array = Eigen::Array; + + using ModeMatOp = SymGEigsCayleyOp; + using Base = HermEigsBase; + using Base::m_nev; + using Base::m_ritz_val; + + const Scalar m_sigma; + + // Set shift and forward + static ModeMatOp set_shift_and_move(ModeMatOp&& op, const Scalar& sigma) + { + if (sigma == Scalar(0)) + throw std::invalid_argument("SymGEigsShiftSolver: sigma cannot be zero in the Cayley mode"); + op.set_shift(sigma); + return std::move(op); + } + + // First transform back the Ritz values, and then sort + void sort_ritzpair(SortRule sort_rule) override + { + // The eigenvalues we get from the iteration is nu = (lambda + sigma) / (lambda - sigma) + // So the eigenvalues of the original problem is lambda = sigma * (nu + 1) / (nu - 1) + m_ritz_val.head(m_nev).array() = m_sigma * (m_ritz_val.head(m_nev).array() + Scalar(1)) / + (m_ritz_val.head(m_nev).array() - Scalar(1)); + Base::sort_ritzpair(sort_rule); + } + +public: + /// + /// Constructor to create a solver object. + /// + /// \param op The matrix operation object that computes \f$y=(A-\sigma B)^{-1}v\f$ + /// for any vector \f$v\f$. Users could either create the object from the + /// wrapper class SymShiftInvert, or define their own that implements all + /// the public members as in SymShiftInvert. + /// \param Bop The \f$B\f$ matrix operation object that implements the matrix-vector + /// multiplication \f$Bv\f$. Users could either create the object from the + /// wrapper classes such as DenseSymMatProd and SparseSymMatProd, or + /// define their own that implements all the public member functions + /// as in DenseSymMatProd. \f$B\f$ needs to be positive definite. + /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, + /// where \f$n\f$ is the size of matrix. + /// \param ncv Parameter that controls the convergence speed of the algorithm. + /// Typically a larger `ncv` means faster convergence, but it may + /// also result in greater memory use and more matrix operations + /// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$, + /// and is advised to take \f$ncv \ge 2\cdot nev\f$. + /// \param sigma The value of the shift. + /// + SymGEigsShiftSolver(OpType& op, BOpType& Bop, Index nev, Index ncv, const Scalar& sigma) : + Base(set_shift_and_move(ModeMatOp(op, Bop), sigma), Bop, nev, ncv), + m_sigma(sigma) + {} +}; + +} // namespace Spectra + +#endif // SPECTRA_SYM_GEIGS_SHIFT_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/SymGEigsSolver.h b/gtsam/3rdparty/Spectra/SymGEigsSolver.h index 68aa37cfc8..caea1657f9 100644 --- a/gtsam/3rdparty/Spectra/SymGEigsSolver.h +++ b/gtsam/3rdparty/Spectra/SymGEigsSolver.h @@ -1,13 +1,13 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SYM_GEIGS_SOLVER_H -#define SYM_GEIGS_SOLVER_H +#ifndef SPECTRA_SYM_GEIGS_SOLVER_H +#define SPECTRA_SYM_GEIGS_SOLVER_H -#include "SymEigsBase.h" +#include "HermEigsBase.h" #include "Util/GEigsMode.h" #include "MatOp/internal/SymGEigsCholeskyOp.h" #include "MatOp/internal/SymGEigsRegInvOp.h" @@ -27,25 +27,21 @@ namespace Spectra { /// matrices, i.e., to solve \f$Ax=\lambda Bx\f$ where \f$A\f$ is symmetric and /// \f$B\f$ is positive definite. /// -/// There are two modes of this solver, specified by the template parameter -/// GEigsMode. See the pages for the specialized classes for details. +/// There are two modes of this solver, specified by the template parameter `Mode`. +/// See the pages for the specialized classes for details. /// - The Cholesky mode assumes that \f$B\f$ can be factorized using Cholesky /// decomposition, which is the preferred mode when the decomposition is /// available. (This can be easily done in Eigen using the dense or sparse /// Cholesky solver.) -/// See \ref SymGEigsSolver "SymGEigsSolver (Cholesky mode)" for more details. +/// See \ref SymGEigsSolver "SymGEigsSolver (Cholesky mode)" for more details. /// - The regular inverse mode requires the matrix-vector product \f$Bv\f$ and the /// linear equation solving operation \f$B^{-1}v\f$. This mode should only be /// used when the Cholesky decomposition of \f$B\f$ is hard to implement, or /// when computing \f$B^{-1}v\f$ is much faster than the Cholesky decomposition. -/// See \ref SymGEigsSolver "SymGEigsSolver (Regular inverse mode)" for more details. +/// See \ref SymGEigsSolver "SymGEigsSolver (Regular inverse mode)" for more details. // Empty class template -template +template class SymGEigsSolver {}; @@ -67,25 +63,17 @@ class SymGEigsSolver /// classes. If the users need to define their own operation classes, then they /// should implement all the public member functions as in those built-in classes. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the requested eigenvalues, for example `LARGEST_MAGN` -/// to retrieve eigenvalues with the largest magnitude. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either -/// use the wrapper classes such as DenseSymMatProd and -/// SparseSymMatProd, or define their -/// own that implements all the public member functions as in -/// DenseSymMatProd. -/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either -/// use the wrapper classes such as DenseCholesky and -/// SparseCholesky, or define their -/// own that implements all the public member functions as in -/// DenseCholesky. -/// \tparam GEigsMode Mode of the generalized eigen solver. In this solver -/// it is Spectra::GEIGS_CHOLESKY. +/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseSymMatProd. +/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either +/// use the wrapper classes such as DenseCholesky and +/// SparseCholesky, or define their own that implements all the +/// public member functions as in DenseCholesky. +/// \tparam Mode Mode of the generalized eigen solver. In this solver +/// it is Spectra::GEigsMode::Cholesky. /// /// Below is an example that demonstrates the usage of this class. /// @@ -112,31 +100,31 @@ class SymGEigsSolver /// // Define the B matrix, a band matrix with 2 on the diagonal and 1 on the subdiagonals /// Eigen::SparseMatrix B(n, n); /// B.reserve(Eigen::VectorXi::Constant(n, 3)); -/// for(int i = 0; i < n; i++) +/// for (int i = 0; i < n; i++) /// { /// B.insert(i, i) = 2.0; -/// if(i > 0) +/// if (i > 0) /// B.insert(i - 1, i) = 1.0; -/// if(i < n - 1) +/// if (i < n - 1) /// B.insert(i + 1, i) = 1.0; /// } /// -/// // Construct matrix operation object using the wrapper classes +/// // Construct matrix operation objects using the wrapper classes /// DenseSymMatProd op(A); /// SparseCholesky Bop(B); /// /// // Construct generalized eigen solver object, requesting the largest three generalized eigenvalues -/// SymGEigsSolver, SparseCholesky, GEIGS_CHOLESKY> -/// geigs(&op, &Bop, 3, 6); +/// SymGEigsSolver, SparseCholesky, GEigsMode::Cholesky> +/// geigs(op, Bop, 3, 6); /// /// // Initialize and compute /// geigs.init(); -/// int nconv = geigs.compute(); +/// int nconv = geigs.compute(SortRule::LargestAlge); /// /// // Retrieve results /// Eigen::VectorXd evalues; /// Eigen::MatrixXd evecs; -/// if(geigs.info() == SUCCESSFUL) +/// if (geigs.info() == CompInfo::Successful) /// { /// evalues = geigs.eigenvalues(); /// evecs = geigs.eigenvectors(); @@ -156,39 +144,39 @@ class SymGEigsSolver /// } /// \endcode -// Partial specialization for GEigsMode = GEIGS_CHOLESKY -template -class SymGEigsSolver : - public SymEigsBase, IdentityBOp> +// Partial specialization for mode = GEigsMode::Cholesky +template +class SymGEigsSolver : + public HermEigsBase, IdentityBOp> { private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; + using Scalar = typename OpType::Scalar; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; - BOpType* m_Bop; + using ModeMatOp = SymGEigsCholeskyOp; + using Base = HermEigsBase; + + const BOpType& m_Bop; public: /// /// Constructor to create a solver object. /// - /// \param op Pointer to the \f$A\f$ matrix operation object. It - /// should implement the matrix-vector multiplication operation of \f$A\f$: + /// \param op The \f$A\f$ matrix operation object that implements the matrix-vector + /// multiplication operation of \f$A\f$: /// calculating \f$Av\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper classes such as DenseSymMatProd, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseSymMatProd. - /// \param Bop Pointer to the \f$B\f$ matrix operation object. It - /// represents a Cholesky decomposition of \f$B\f$, and should - /// implement the lower and upper triangular solving operations: + /// \param Bop The \f$B\f$ matrix operation object that represents a Cholesky decomposition of \f$B\f$. + /// It should implement the lower and upper triangular solving operations: /// calculating \f$L^{-1}v\f$ and \f$(L')^{-1}v\f$ for any vector /// \f$v\f$, where \f$LL'=B\f$. Users could either /// create the object from the wrapper classes such as DenseCholesky, or /// define their own that implements all the public member functions - /// as in DenseCholesky. + /// as in DenseCholesky. \f$B\f$ needs to be positive definite. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, /// where \f$n\f$ is the size of matrix. /// \param ncv Parameter that controls the convergence speed of the algorithm. @@ -197,37 +185,30 @@ class SymGEigsSolver : /// in each iteration. This parameter must satisfy \f$nev < ncv \le n\f$, /// and is advised to take \f$ncv \ge 2\cdot nev\f$. /// - SymGEigsSolver(OpType* op, BOpType* Bop, Index nev, Index ncv) : - SymEigsBase, IdentityBOp>( - new SymGEigsCholeskyOp(*op, *Bop), NULL, nev, ncv), + SymGEigsSolver(OpType& op, BOpType& Bop, Index nev, Index ncv) : + Base(ModeMatOp(op, Bop), IdentityBOp(), nev, ncv), m_Bop(Bop) {} /// \cond - ~SymGEigsSolver() - { - // m_op contains the constructed SymGEigsCholeskyOp object - delete this->m_op; - } - - Matrix eigenvectors(Index nvec) const + Matrix eigenvectors(Index nvec) const override { - Matrix res = SymEigsBase, IdentityBOp>::eigenvectors(nvec); + Matrix res = Base::eigenvectors(nvec); Vector tmp(res.rows()); const Index nconv = res.cols(); for (Index i = 0; i < nconv; i++) { - m_Bop->upper_triangular_solve(&res(0, i), tmp.data()); + m_Bop.upper_triangular_solve(&res(0, i), tmp.data()); res.col(i).noalias() = tmp; } return res; } - Matrix eigenvectors() const + Matrix eigenvectors() const override { - return SymGEigsSolver::eigenvectors(this->m_nev); + return SymGEigsSolver::eigenvectors(this->m_nev); } /// \endcond @@ -252,53 +233,45 @@ class SymGEigsSolver : /// is always preferred. If the users need to define their own operation classes, then they /// should implement all the public member functions as in those built-in classes. /// -/// \tparam Scalar The element type of the matrix. -/// Currently supported types are `float`, `double` and `long double`. -/// \tparam SelectionRule An enumeration value indicating the selection rule of -/// the requested eigenvalues, for example `LARGEST_MAGN` -/// to retrieve eigenvalues with the largest magnitude. -/// The full list of enumeration values can be found in -/// \ref Enumerations. -/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either -/// use the wrapper classes such as DenseSymMatProd and -/// SparseSymMatProd, or define their -/// own that implements all the public member functions as in -/// DenseSymMatProd. -/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either -/// use the wrapper class SparseRegularInverse, or define their -/// own that implements all the public member functions as in -/// SparseRegularInverse. -/// \tparam GEigsMode Mode of the generalized eigen solver. In this solver -/// it is Spectra::GEIGS_REGULAR_INVERSE. +/// \tparam OpType The name of the matrix operation class for \f$A\f$. Users could either +/// use the wrapper classes such as DenseSymMatProd and +/// SparseSymMatProd, or define their own that implements the type +/// definition `Scalar` and all the public member functions as in +/// DenseSymMatProd. +/// \tparam BOpType The name of the matrix operation class for \f$B\f$. Users could either +/// use the wrapper class SparseRegularInverse, or define their +/// own that implements all the public member functions as in +/// SparseRegularInverse. +/// \tparam Mode Mode of the generalized eigen solver. In this solver +/// it is Spectra::GEigsMode::RegularInverse. /// -// Partial specialization for GEigsMode = GEIGS_REGULAR_INVERSE -template -class SymGEigsSolver : - public SymEigsBase, BOpType> +// Partial specialization for mode = GEigsMode::RegularInverse +template +class SymGEigsSolver : + public HermEigsBase, BOpType> { private: - typedef Eigen::Index Index; + using Index = Eigen::Index; + + using ModeMatOp = SymGEigsRegInvOp; + using Base = HermEigsBase; public: /// /// Constructor to create a solver object. /// - /// \param op Pointer to the \f$A\f$ matrix operation object. It - /// should implement the matrix-vector multiplication operation of \f$A\f$: + /// \param op The \f$A\f$ matrix operation object that implements the matrix-vector + /// multiplication operation of \f$A\f$: /// calculating \f$Av\f$ for any vector \f$v\f$. Users could either /// create the object from the wrapper classes such as DenseSymMatProd, or - /// define their own that implements all the public member functions + /// define their own that implements all the public members /// as in DenseSymMatProd. - /// \param Bop Pointer to the \f$B\f$ matrix operation object. It should - /// implement the multiplication operation \f$Bv\f$ and the linear equation - /// solving operation \f$B^{-1}v\f$ for any vector \f$v\f$. Users could either - /// create the object from the wrapper class SparseRegularInverse, or + /// \param Bop The \f$B\f$ matrix operation object that implements the multiplication operation + /// \f$Bv\f$ and the linear equation solving operation \f$B^{-1}v\f$ for any vector \f$v\f$. + /// Users could either create the object from the wrapper class SparseRegularInverse, or /// define their own that implements all the public member functions - /// as in SparseRegularInverse. + /// as in SparseRegularInverse. \f$B\f$ needs to be positive definite. /// \param nev Number of eigenvalues requested. This should satisfy \f$1\le nev \le n-1\f$, /// where \f$n\f$ is the size of matrix. /// \param ncv Parameter that controls the convergence speed of the algorithm. @@ -307,20 +280,11 @@ class SymGEigsSolver, BOpType>( - new SymGEigsRegInvOp(*op, *Bop), Bop, nev, ncv) + SymGEigsSolver(OpType& op, BOpType& Bop, Index nev, Index ncv) : + Base(ModeMatOp(op, Bop), Bop, nev, ncv) {} - - /// \cond - ~SymGEigsSolver() - { - // m_op contains the constructed SymGEigsRegInvOp object - delete this->m_op; - } - /// \endcond }; } // namespace Spectra -#endif // SYM_GEIGS_SOLVER_H +#endif // SPECTRA_SYM_GEIGS_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/Util/CompInfo.h b/gtsam/3rdparty/Spectra/Util/CompInfo.h index 07b8399a16..bd2108ad18 100644 --- a/gtsam/3rdparty/Spectra/Util/CompInfo.h +++ b/gtsam/3rdparty/Spectra/Util/CompInfo.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef COMP_INFO_H -#define COMP_INFO_H +#ifndef SPECTRA_COMP_INFO_H +#define SPECTRA_COMP_INFO_H namespace Spectra { @@ -14,22 +14,23 @@ namespace Spectra { /// /// The enumeration to report the status of computation. /// -enum COMPUTATION_INFO +enum class CompInfo { - SUCCESSFUL = 0, ///< Computation was successful. + Successful, ///< Computation was successful. - NOT_COMPUTED, ///< Used in eigen solvers, indicating that computation - ///< has not been conducted. Users should call - ///< the `compute()` member function of solvers. + NotComputed, ///< Used in eigen solvers, indicating that computation + ///< has not been conducted. Users should call + ///< the `compute()` member function of solvers. - NOT_CONVERGING, ///< Used in eigen solvers, indicating that some eigenvalues - ///< did not converge. The `compute()` - ///< function returns the number of converged eigenvalues. + NotConverging, ///< Used in eigen solvers, indicating that some eigenvalues + ///< did not converge. The `compute()` + ///< function returns the number of converged eigenvalues. - NUMERICAL_ISSUE ///< Used in Cholesky decomposition, indicating that the - ///< matrix is not positive definite. + NumericalIssue ///< Used in various matrix factorization classes, for example in + ///< Cholesky decomposition it indicates that the + ///< matrix is not positive definite. }; } // namespace Spectra -#endif // COMP_INFO_H +#endif // SPECTRA_COMP_INFO_H diff --git a/gtsam/3rdparty/Spectra/Util/GEigsMode.h b/gtsam/3rdparty/Spectra/Util/GEigsMode.h index a547ac0bf1..5c622ac9aa 100644 --- a/gtsam/3rdparty/Spectra/Util/GEigsMode.h +++ b/gtsam/3rdparty/Spectra/Util/GEigsMode.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef GEIGS_MODE_H -#define GEIGS_MODE_H +#ifndef SPECTRA_GEIGS_MODE_H +#define SPECTRA_GEIGS_MODE_H namespace Spectra { @@ -14,19 +14,15 @@ namespace Spectra { /// /// The enumeration to specify the mode of generalized eigenvalue solver. /// -enum GEIGS_MODE +enum class GEigsMode { - GEIGS_CHOLESKY = 0, ///< Using Cholesky decomposition to solve generalized eigenvalues. - - GEIGS_REGULAR_INVERSE, ///< Regular inverse mode for generalized eigenvalue solver. - - GEIGS_SHIFT_INVERT, ///< Shift-and-invert mode for generalized eigenvalue solver. - - GEIGS_BUCKLING, ///< Buckling mode for generalized eigenvalue solver. - - GEIGS_CAYLEY ///< Cayley transformation mode for generalized eigenvalue solver. + Cholesky, ///< Using Cholesky decomposition to solve generalized eigenvalues. + RegularInverse, ///< Regular inverse mode for generalized eigenvalue solver. + ShiftInvert, ///< Shift-and-invert mode for generalized eigenvalue solver. + Buckling, ///< Buckling mode for generalized eigenvalue solver. + Cayley ///< Cayley transformation mode for generalized eigenvalue solver. }; } // namespace Spectra -#endif // GEIGS_MODE_H +#endif // SPECTRA_GEIGS_MODE_H diff --git a/gtsam/3rdparty/Spectra/Util/SelectionRule.h b/gtsam/3rdparty/Spectra/Util/SelectionRule.h index 237950b4d7..d67da6d0bf 100644 --- a/gtsam/3rdparty/Spectra/Util/SelectionRule.h +++ b/gtsam/3rdparty/Spectra/Util/SelectionRule.h @@ -1,11 +1,11 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SELECTION_RULE_H -#define SELECTION_RULE_H +#ifndef SPECTRA_SELECTION_RULE_H +#define SPECTRA_SELECTION_RULE_H #include // std::vector #include // std::abs @@ -14,10 +14,13 @@ #include // std::pair #include // std::invalid_argument +#include +#include "TypeTraits.h" + namespace Spectra { /// -/// \defgroup Enumerations +/// \defgroup Enumerations Enumerations /// /// Enumeration types for the selection rule of eigenvalues. /// @@ -27,81 +30,46 @@ namespace Spectra { /// /// The enumeration of selection rules of desired eigenvalues. /// -enum SELECT_EIGENVALUE +enum class SortRule { - LARGEST_MAGN = 0, ///< Select eigenvalues with largest magnitude. Magnitude - ///< means the absolute value for real numbers and norm for - ///< complex numbers. Applies to both symmetric and general - ///< eigen solvers. - - LARGEST_REAL, ///< Select eigenvalues with largest real part. Only for general eigen solvers. + LargestMagn, ///< Select eigenvalues with largest magnitude. Magnitude + ///< means the absolute value for real numbers and norm for + ///< complex numbers. Applies to both symmetric and general + ///< eigen solvers. - LARGEST_IMAG, ///< Select eigenvalues with largest imaginary part (in magnitude). Only for general eigen solvers. + LargestReal, ///< Select eigenvalues with largest real part. Only for general eigen solvers. - LARGEST_ALGE, ///< Select eigenvalues with largest algebraic value, considering - ///< any negative sign. Only for symmetric eigen solvers. + LargestImag, ///< Select eigenvalues with largest imaginary part (in magnitude). Only for general eigen solvers. - SMALLEST_MAGN, ///< Select eigenvalues with smallest magnitude. Applies to both symmetric and general - ///< eigen solvers. + LargestAlge, ///< Select eigenvalues with largest algebraic value, considering + ///< any negative sign. Only for symmetric eigen solvers. - SMALLEST_REAL, ///< Select eigenvalues with smallest real part. Only for general eigen solvers. + SmallestMagn, ///< Select eigenvalues with smallest magnitude. Applies to both symmetric and general + ///< eigen solvers. - SMALLEST_IMAG, ///< Select eigenvalues with smallest imaginary part (in magnitude). Only for general eigen solvers. + SmallestReal, ///< Select eigenvalues with smallest real part. Only for general eigen solvers. - SMALLEST_ALGE, ///< Select eigenvalues with smallest algebraic value. Only for symmetric eigen solvers. + SmallestImag, ///< Select eigenvalues with smallest imaginary part (in magnitude). Only for general eigen solvers. - BOTH_ENDS ///< Select eigenvalues half from each end of the spectrum. When - ///< `nev` is odd, compute more from the high end. Only for symmetric eigen solvers. -}; + SmallestAlge, ///< Select eigenvalues with smallest algebraic value. Only for symmetric eigen solvers. -/// -/// \ingroup Enumerations -/// -/// The enumeration of selection rules of desired eigenvalues. Alias for `SELECT_EIGENVALUE`. -/// -enum SELECT_EIGENVALUE_ALIAS -{ - WHICH_LM = 0, ///< Alias for `LARGEST_MAGN` - WHICH_LR, ///< Alias for `LARGEST_REAL` - WHICH_LI, ///< Alias for `LARGEST_IMAG` - WHICH_LA, ///< Alias for `LARGEST_ALGE` - WHICH_SM, ///< Alias for `SMALLEST_MAGN` - WHICH_SR, ///< Alias for `SMALLEST_REAL` - WHICH_SI, ///< Alias for `SMALLEST_IMAG` - WHICH_SA, ///< Alias for `SMALLEST_ALGE` - WHICH_BE ///< Alias for `BOTH_ENDS` + BothEnds ///< Select eigenvalues half from each end of the spectrum. When + ///< `nev` is odd, compute more from the high end. Only for symmetric eigen solvers. }; /// \cond -// Get the element type of a "scalar" -// ElemType => double -// ElemType< std::complex > => double -template -class ElemType -{ -public: - typedef T type; -}; - -template -class ElemType > -{ -public: - typedef T type; -}; - -// When comparing eigenvalues, we first calculate the "target" -// to sort. For example, if we want to choose the eigenvalues with +// When comparing eigenvalues, we first calculate the "target" to sort. +// For example, if we want to choose the eigenvalues with // largest magnitude, the target will be -abs(x). // The minus sign is due to the fact that std::sort() sorts in ascending order. // Default target: throw an exception -template +template class SortingTarget { public: - static typename ElemType::type get(const Scalar& val) + static ElemType get(const Scalar& val) { using std::abs; throw std::invalid_argument("incompatible selection rule"); @@ -109,23 +77,23 @@ class SortingTarget } }; -// Specialization for LARGEST_MAGN -// This covers [float, double, complex] x [LARGEST_MAGN] +// Specialization for SortRule::LargestMagn +// This covers [float, double, complex] x [SortRule::LargestMagn] template -class SortingTarget +class SortingTarget { public: - static typename ElemType::type get(const Scalar& val) + static ElemType get(const Scalar& val) { using std::abs; return -abs(val); } }; -// Specialization for LARGEST_REAL -// This covers [complex] x [LARGEST_REAL] +// Specialization for SortRule::LargestReal +// This covers [complex] x [SortRule::LargestReal] template -class SortingTarget, LARGEST_REAL> +class SortingTarget, SortRule::LargestReal> { public: static RealType get(const std::complex& val) @@ -134,10 +102,10 @@ class SortingTarget, LARGEST_REAL> } }; -// Specialization for LARGEST_IMAG -// This covers [complex] x [LARGEST_IMAG] +// Specialization for SortRule::LargestImag +// This covers [complex] x [SortRule::LargestImag] template -class SortingTarget, LARGEST_IMAG> +class SortingTarget, SortRule::LargestImag> { public: static RealType get(const std::complex& val) @@ -147,10 +115,10 @@ class SortingTarget, LARGEST_IMAG> } }; -// Specialization for LARGEST_ALGE -// This covers [float, double] x [LARGEST_ALGE] +// Specialization for SortRule::LargestAlge +// This covers [float, double] x [SortRule::LargestAlge] template -class SortingTarget +class SortingTarget { public: static Scalar get(const Scalar& val) @@ -159,12 +127,12 @@ class SortingTarget } }; -// Here BOTH_ENDS is the same as LARGEST_ALGE, but +// Here SortRule::BothEnds is the same as SortRule::LargestAlge, but // we need some additional steps, which are done in // SymEigsSolver.h => retrieve_ritzpair(). // There we move the smallest values to the proper locations. template -class SortingTarget +class SortingTarget { public: static Scalar get(const Scalar& val) @@ -173,23 +141,23 @@ class SortingTarget } }; -// Specialization for SMALLEST_MAGN -// This covers [float, double, complex] x [SMALLEST_MAGN] +// Specialization for SortRule::SmallestMagn +// This covers [float, double, complex] x [SortRule::SmallestMagn] template -class SortingTarget +class SortingTarget { public: - static typename ElemType::type get(const Scalar& val) + static ElemType get(const Scalar& val) { using std::abs; return abs(val); } }; -// Specialization for SMALLEST_REAL -// This covers [complex] x [SMALLEST_REAL] +// Specialization for SortRule::SmallestReal +// This covers [complex] x [SortRule::SmallestReal] template -class SortingTarget, SMALLEST_REAL> +class SortingTarget, SortRule::SmallestReal> { public: static RealType get(const std::complex& val) @@ -198,10 +166,10 @@ class SortingTarget, SMALLEST_REAL> } }; -// Specialization for SMALLEST_IMAG -// This covers [complex] x [SMALLEST_IMAG] +// Specialization for SortRule::SmallestImag +// This covers [complex] x [SortRule::SmallestImag] template -class SortingTarget, SMALLEST_IMAG> +class SortingTarget, SortRule::SmallestImag> { public: static RealType get(const std::complex& val) @@ -211,10 +179,10 @@ class SortingTarget, SMALLEST_IMAG> } }; -// Specialization for SMALLEST_ALGE -// This covers [float, double] x [SMALLEST_ALGE] +// Specialization for SortRule::SmallestAlge +// This covers [float, double] x [SortRule::SmallestAlge] template -class SortingTarget +class SortingTarget { public: static Scalar get(const Scalar& val) @@ -223,53 +191,110 @@ class SortingTarget } }; -// Sort eigenvalues and return the order index -template -class PairComparator +// Sort eigenvalues +template +class SortEigenvalue { +private: + using Index = Eigen::Index; + using IndexArray = std::vector; + + const T* m_evals; + IndexArray m_index; + public: - bool operator()(const PairType& v1, const PairType& v2) + // Sort indices according to the eigenvalues they point to + inline bool operator()(Index i, Index j) { - return v1.first < v2.first; + return SortingTarget::get(m_evals[i]) < SortingTarget::get(m_evals[j]); } + + SortEigenvalue(const T* start, Index size) : + m_evals(start), m_index(size) + { + for (Index i = 0; i < size; i++) + { + m_index[i] = i; + } + std::sort(m_index.begin(), m_index.end(), *this); + } + + inline IndexArray index() const { return m_index; } + inline void swap(IndexArray& other) { m_index.swap(other); } }; -template -class SortEigenvalue +// Sort values[:len] according to the selection rule, and return the indices +template +std::vector argsort(SortRule selection, const Eigen::Matrix& values, Eigen::Index len) { -private: - typedef typename ElemType::type TargetType; // Type of the sorting target, will be - // a floating number type, e.g. "double" - typedef std::pair PairType; // Type of the sorting pair, including - // the sorting target and the index + using Index = Eigen::Index; - std::vector pair_sort; - -public: - SortEigenvalue(const T* start, int size) : - pair_sort(size) + // Sort Ritz values and put the wanted ones at the beginning + std::vector ind; + switch (selection) { - for (int i = 0; i < size; i++) + case SortRule::LargestMagn: + { + SortEigenvalue sorting(values.data(), len); + sorting.swap(ind); + break; + } + case SortRule::BothEnds: + case SortRule::LargestAlge: + { + SortEigenvalue sorting(values.data(), len); + sorting.swap(ind); + break; + } + case SortRule::SmallestMagn: + { + SortEigenvalue sorting(values.data(), len); + sorting.swap(ind); + break; + } + case SortRule::SmallestAlge: { - pair_sort[i].first = SortingTarget::get(start[i]); - pair_sort[i].second = i; + SortEigenvalue sorting(values.data(), len); + sorting.swap(ind); + break; } - PairComparator comp; - std::sort(pair_sort.begin(), pair_sort.end(), comp); + default: + throw std::invalid_argument("unsupported selection rule"); } - std::vector index() + // For SortRule::BothEnds, the eigenvalues are sorted according to the + // SortRule::LargestAlge rule, so we need to move those smallest values to the left + // The order would be + // Largest => Smallest => 2nd largest => 2nd smallest => ... + // We keep this order since the first k values will always be + // the wanted collection, no matter k is nev_updated (used in SymEigsBase::restart()) + // or is nev (used in SymEigsBase::sort_ritzpair()) + if (selection == SortRule::BothEnds) { - std::vector ind(pair_sort.size()); - for (unsigned int i = 0; i < ind.size(); i++) - ind[i] = pair_sort[i].second; - - return ind; + std::vector ind_copy(ind); + for (Index i = 0; i < len; i++) + { + // If i is even, pick values from the left (large values) + // If i is odd, pick values from the right (small values) + if (i % 2 == 0) + ind[i] = ind_copy[i / 2]; + else + ind[i] = ind_copy[len - 1 - i / 2]; + } } -}; + + return ind; +} + +// Default vector length +template +std::vector argsort(SortRule selection, const Eigen::Matrix& values) +{ + return argsort(selection, values, values.size()); +} /// \endcond } // namespace Spectra -#endif // SELECTION_RULE_H +#endif // SPECTRA_SELECTION_RULE_H diff --git a/gtsam/3rdparty/Spectra/Util/SimpleRandom.h b/gtsam/3rdparty/Spectra/Util/SimpleRandom.h index 83fa7c86fa..0b7dda758d 100644 --- a/gtsam/3rdparty/Spectra/Util/SimpleRandom.h +++ b/gtsam/3rdparty/Spectra/Util/SimpleRandom.h @@ -1,13 +1,14 @@ -// Copyright (C) 2016-2019 Yixuan Qiu +// Copyright (C) 2016-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef SIMPLE_RANDOM_H -#define SIMPLE_RANDOM_H +#ifndef SPECTRA_SIMPLE_RANDOM_H +#define SPECTRA_SIMPLE_RANDOM_H #include +#include /// \cond @@ -25,61 +26,98 @@ namespace Spectra { // 5. Based on public domain code by Ray Gardner // http://stjarnhimlen.se/snippets/rg_rand.c -template -class SimpleRandom +// Given a 32-bit integer as the seed, generate a pseudo random 32-bit integer +inline long next_long_rand(long seed) { -private: - typedef Eigen::Index Index; - typedef Eigen::Matrix Vector; + constexpr unsigned int m_a = 16807; // multiplier + constexpr unsigned long m_max = 2147483647L; // 2^31 - 1 - const unsigned int m_a; // multiplier - const unsigned long m_max; // 2^31 - 1 - long m_rand; + unsigned long lo, hi; - inline long next_long_rand(long seed) + lo = m_a * (long) (seed & 0xFFFF); + hi = m_a * (long) ((unsigned long) seed >> 16); + lo += (hi & 0x7FFF) << 16; + if (lo > m_max) + { + lo &= m_max; + ++lo; + } + lo += hi >> 15; + if (lo > m_max) { - unsigned long lo, hi; + lo &= m_max; + ++lo; + } + return (long) lo; +} - lo = m_a * (long) (seed & 0xFFFF); - hi = m_a * (long) ((unsigned long) seed >> 16); - lo += (hi & 0x7FFF) << 16; - if (lo > m_max) - { - lo &= m_max; - ++lo; - } - lo += hi >> 15; - if (lo > m_max) - { - lo &= m_max; - ++lo; - } - return (long) lo; +// Generate a random scalar from the given random seed +// Also overwrite seed with the new random integer +template +struct RandomScalar +{ + static Scalar run(long& seed) + { + constexpr unsigned long m_max = 2147483647L; // 2^31 - 1 + + seed = next_long_rand(seed); + return Scalar(seed) / Scalar(m_max) - Scalar(0.5); + } +}; +// Specialization for complex values +template +struct RandomScalar> +{ + static std::complex run(long& seed) + { + RealScalar r = RandomScalar::run(seed); + RealScalar i = RandomScalar::run(seed); + return std::complex(r, i); } +}; + +// A simple random generator class +template +class SimpleRandom +{ +private: + // The real part type of the matrix element + using RealScalar = typename Eigen::NumTraits::Real; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + + long m_rand; // RNG state public: - SimpleRandom(unsigned long init_seed) : - m_a(16807), - m_max(2147483647L), - m_rand(init_seed ? (init_seed & m_max) : 1) - {} + SimpleRandom(unsigned long init_seed) + { + constexpr unsigned long m_max = 2147483647L; // 2^31 - 1 + m_rand = init_seed ? (init_seed & m_max) : 1; + } + // Return a single random number, ranging from -0.5 to 0.5 Scalar random() { - m_rand = next_long_rand(m_rand); - return Scalar(m_rand) / Scalar(m_max) - Scalar(0.5); + return RandomScalar::run(m_rand); } - // Vector of random numbers of type Scalar + // Fill the given vector with random numbers // Ranging from -0.5 to 0.5 - Vector random_vec(const Index len) + void random_vec(Vector& vec) { - Vector res(len); + const Index len = vec.size(); for (Index i = 0; i < len; i++) { - m_rand = next_long_rand(m_rand); - res[i] = Scalar(m_rand) / Scalar(m_max) - Scalar(0.5); + vec[i] = random(); } + } + + // Return a vector of random numbers + // Ranging from -0.5 to 0.5 + Vector random_vec(const Index len) + { + Vector res(len); + random_vec(res); return res; } }; @@ -88,4 +126,4 @@ class SimpleRandom /// \endcond -#endif // SIMPLE_RANDOM_H +#endif // SPECTRA_SIMPLE_RANDOM_H diff --git a/gtsam/3rdparty/Spectra/Util/TypeTraits.h b/gtsam/3rdparty/Spectra/Util/TypeTraits.h index 29288c5a62..a413f00160 100644 --- a/gtsam/3rdparty/Spectra/Util/TypeTraits.h +++ b/gtsam/3rdparty/Spectra/Util/TypeTraits.h @@ -1,17 +1,23 @@ -// Copyright (C) 2018-2019 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef TYPE_TRAITS_H -#define TYPE_TRAITS_H +#ifndef SPECTRA_TYPE_TRAITS_H +#define SPECTRA_TYPE_TRAITS_H #include #include /// \cond +// Clang-Format will have unintended effects: +// static constexpr Scalar(min)() +// So we turn it off here +// +// clang-format off + namespace Spectra { // For a real value type "Scalar", we want to know its smallest @@ -30,9 +36,13 @@ namespace Spectra { template struct TypeTraits { - static inline Scalar min() + static constexpr Scalar epsilon() + { + return Eigen::numext::numeric_limits::epsilon(); + } + static constexpr Scalar (min)() { - return Eigen::numext::pow(Eigen::NumTraits::epsilon(), Scalar(3)); + return epsilon() * epsilon() * epsilon(); } }; @@ -40,32 +50,50 @@ struct TypeTraits template <> struct TypeTraits { - static inline float min() + static constexpr float epsilon() + { + return std::numeric_limits::epsilon(); + } + static constexpr float (min)() { - return std::numeric_limits::min(); + return (std::numeric_limits::min)(); } }; template <> struct TypeTraits { - static inline double min() + static constexpr double epsilon() { - return std::numeric_limits::min(); + return std::numeric_limits::epsilon(); + } + static constexpr double (min)() + { + return (std::numeric_limits::min)(); } }; template <> struct TypeTraits { - static inline long double min() + static constexpr long double epsilon() { - return std::numeric_limits::min(); + return std::numeric_limits::epsilon(); + } + static constexpr long double (min)() + { + return (std::numeric_limits::min)(); } }; +// Get the element type of a "scalar" +// ElemType => double +// ElemType> => double +template +using ElemType = typename Eigen::NumTraits::Real; + } // namespace Spectra /// \endcond -#endif // TYPE_TRAITS_H +#endif // SPECTRA_TYPE_TRAITS_H diff --git a/gtsam/3rdparty/Spectra/Util/Version.h b/gtsam/3rdparty/Spectra/Util/Version.h new file mode 100644 index 0000000000..39b267c6c1 --- /dev/null +++ b/gtsam/3rdparty/Spectra/Util/Version.h @@ -0,0 +1,16 @@ +// Copyright (C) 2020-2025 Yixuan Qiu +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at https://mozilla.org/MPL/2.0/. + +#ifndef SPECTRA_VERSION_H +#define SPECTRA_VERSION_H + +#define SPECTRA_MAJOR_VERSION 1 +#define SPECTRA_MINOR_VERSION 1 +#define SPECTRA_PATCH_VERSION 0 + +#define SPECTRA_VERSION (SPECTRA_MAJOR_VERSION * 10000 + SPECTRA_MINOR_VERSION * 100 + SPECTRA_PATCH_VERSION) + +#endif // SPECTRA_VERSION_H diff --git a/gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h b/gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h index 69c4d92c0d..7002324509 100644 --- a/gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h +++ b/gtsam/3rdparty/Spectra/contrib/LOBPCGSolver.h @@ -1,552 +1,551 @@ -// Written by Anna Araslanova -// Modified by Yixuan Qiu -// License: MIT - -#ifndef LOBPCG_SOLVER -#define LOBPCG_SOLVER - -#include -#include - -#include -#include -#include -#include -#include - -#include "../SymGEigsSolver.h" - -namespace Spectra { - -/// -/// \ingroup EigenSolver -/// - -/// *** METHOD -/// The class represent the LOBPCG algorithm, which was invented by Andrew Knyazev -/// Theoretical background of the procedure can be found in the articles below -/// - Knyazev, A.V., 2001. Toward the optimal preconditioned eigensolver : Locally optimal block preconditioned conjugate gradient method.SIAM journal on scientific computing, 23(2), pp.517 - 541. -/// - Knyazev, A.V., Argentati, M.E., Lashuk, I. and Ovtchinnikov, E.E., 2007. Block locally optimal preconditioned eigenvalue xolvers(BLOPEX) in HYPRE and PETSc.SIAM Journal on Scientific Computing, 29(5), pp.2224 - 2239. -/// -/// *** CONDITIONS OF USE -/// Locally Optimal Block Preconditioned Conjugate Gradient(LOBPCG) is a method for finding the M smallest eigenvalues -/// and eigenvectors of a large symmetric positive definite generalized eigenvalue problem -/// \f$Ax=\lambda Bx,\f$ -/// where \f$A_{NxN}\f$ is a symmetric matrix, \f$B\f$ is symmetric and positive - definite. \f$A and B\f$ are also assumed large and sparse -/// \f$\textit{M}\f$ should be \f$\<< textit{N}\f$ (at least \f$\textit{5M} < \textit{N} \f$) -/// -/// *** ARGUMENTS -/// Eigen::SparseMatrix A; // N*N - Ax = lambda*Bx, lrage and sparse -/// Eigen::SparseMatrix X; // N*M - initial approximations to eigenvectors (random in general case) -/// Spectra::LOBPCGSolver solver(A, X); -/// *Eigen::SparseMatrix B; // N*N - Ax = lambda*Bx, sparse, positive definite -/// solver.setConstraints(B); -/// *Eigen::SparseMatrix Y; // N*K - constraints, already found eigenvectors -/// solver.setB(B); -/// *Eigen::SparseMatrix T; // N*N - preconditioner ~ A^-1 -/// solver.setPreconditioner(T); -/// -/// *** OUTCOMES -/// solver.solve(); // compute eigenpairs // void -/// solver.info(); // state of converjance // int -/// solver.residuals(); // get residuals to evaluate biases // Eigen::Matrix -/// solver.eigenvalues(); // get eigenvalues // Eigen::Matrix -/// solver.eigenvectors(); // get eigenvectors // Eigen::Matrix -/// -/// *** EXAMPLE -/// \code{.cpp} -/// #include -/// -/// // random A -/// Matrix a; -/// a = (Matrix::Random(10, 10).array() > 0.6).cast() * Matrix::Random(10, 10).array() * 5; -/// a = Matrix((a).triangularView()) + Matrix((a).triangularView()).transpose(); -/// for (int i = 0; i < 10; i++) -/// a(i, i) = i + 0.5; -/// std::cout << a << "\n"; -/// Eigen::SparseMatrix A(a.sparseView()); -/// // random X -/// Eigen::Matrix x; -/// x = Matrix::Random(10, 2).array(); -/// Eigen::SparseMatrix X(x.sparseView()); -/// // solve Ax = lambda*x -/// Spectra::LOBPCGSolver solver(A, X); -/// solver.compute(10, 1e-4); // 10 iterations, L2_tolerance = 1e-4*N -/// std::cout << "info\n" << solver.info() << std::endl; -/// std::cout << "eigenvalues\n" << solver.eigenvalues() << std::endl; -/// std::cout << "eigenvectors\n" << solver.eigenvectors() << std::endl; -/// std::cout << "residuals\n" << solver.residuals() << std::endl; -/// \endcode -/// - -template -class LOBPCGSolver -{ -private: - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - - typedef std::complex Complex; - typedef Eigen::Matrix ComplexMatrix; - typedef Eigen::Matrix ComplexVector; - - typedef Eigen::SparseMatrix SparseMatrix; - typedef Eigen::SparseMatrix SparseComplexMatrix; - - const int m_n; // dimension of matrix A - const int m_nev; // number of eigenvalues requested - SparseMatrix A, X; - SparseMatrix m_Y, m_B, m_preconditioner; - bool flag_with_constraints, flag_with_B, flag_with_preconditioner; - -public: - SparseMatrix m_residuals; - Matrix m_evectors; - Vector m_evalues; - int m_info; - -private: - // B-orthonormalize matrix M - int orthogonalizeInPlace(SparseMatrix& M, SparseMatrix& B, - SparseMatrix& true_BM, bool has_true_BM = false) - { - SparseMatrix BM; - - if (has_true_BM == false) - { - if (flag_with_B) - { - BM = B * M; - } - else - { - BM = M; - } - } - else - { - BM = true_BM; - } - - Eigen::SimplicialLDLT chol_MBM(M.transpose() * BM); - - if (chol_MBM.info() != SUCCESSFUL) - { - // LDLT decomposition fail - m_info = chol_MBM.info(); - return chol_MBM.info(); - } - - SparseComplexMatrix Upper_MBM = chol_MBM.matrixU().template cast(); - ComplexVector D_MBM_vec = chol_MBM.vectorD().template cast(); - - D_MBM_vec = D_MBM_vec.cwiseSqrt(); - - for (int i = 0; i < D_MBM_vec.rows(); i++) - { - D_MBM_vec(i) = Complex(1.0, 0.0) / D_MBM_vec(i); - } - - SparseComplexMatrix D_MBM_mat(D_MBM_vec.asDiagonal()); - - SparseComplexMatrix U_inv(Upper_MBM.rows(), Upper_MBM.cols()); - U_inv.setIdentity(); - Upper_MBM.template triangularView().solveInPlace(U_inv); - - SparseComplexMatrix right_product = U_inv * D_MBM_mat; - M = M * right_product.real(); - if (flag_with_B) - { - true_BM = B * M; - } - else - { - true_BM = M; - } - - return SUCCESSFUL; - } - - void applyConstraintsInPlace(SparseMatrix& X, SparseMatrix& Y, - SparseMatrix& B) - { - SparseMatrix BY; - if (flag_with_B) - { - BY = B * Y; - } - else - { - BY = Y; - } - - SparseMatrix YBY = Y.transpose() * BY; - SparseMatrix BYX = BY.transpose() * X; - - SparseMatrix YBY_XYX = (Matrix(YBY).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Matrix(BYX))).sparseView(); - X = X - Y * YBY_XYX; - } - - /* - return - 'AB - CD' - */ - Matrix stack_4_matricies(Matrix A, Matrix B, - Matrix C, Matrix D) - { - Matrix result(A.rows() + C.rows(), A.cols() + B.cols()); - result.topLeftCorner(A.rows(), A.cols()) = A; - result.topRightCorner(B.rows(), B.cols()) = B; - result.bottomLeftCorner(C.rows(), C.cols()) = C; - result.bottomRightCorner(D.rows(), D.cols()) = D; - return result; - } - - Matrix stack_9_matricies(Matrix A, Matrix B, Matrix C, - Matrix D, Matrix E, Matrix F, - Matrix G, Matrix H, Matrix I) - { - Matrix result(A.rows() + D.rows() + G.rows(), A.cols() + B.cols() + C.cols()); - result.block(0, 0, A.rows(), A.cols()) = A; - result.block(0, A.cols(), B.rows(), B.cols()) = B; - result.block(0, A.cols() + B.cols(), C.rows(), C.cols()) = C; - result.block(A.rows(), 0, D.rows(), D.cols()) = D; - result.block(A.rows(), A.cols(), E.rows(), E.cols()) = E; - result.block(A.rows(), A.cols() + B.cols(), F.rows(), F.cols()) = F; - result.block(A.rows() + D.rows(), 0, G.rows(), G.cols()) = G; - result.block(A.rows() + D.rows(), A.cols(), H.rows(), H.cols()) = H; - result.block(A.rows() + D.rows(), A.cols() + B.cols(), I.rows(), I.cols()) = I; - - return result; - } - - void sort_epairs(Vector& evalues, Matrix& evectors, int SelectionRule) - { - std::function cmp; - if (SelectionRule == SMALLEST_ALGE) - cmp = std::less{}; - else - cmp = std::greater{}; - - std::map epairs(cmp); - for (int i = 0; i < m_evectors.cols(); ++i) - epairs.insert(std::make_pair(evalues(i), evectors.col(i))); - - int i = 0; - for (auto& epair : epairs) - { - evectors.col(i) = epair.second; - evalues(i) = epair.first; - i++; - } - } - - void removeColumns(SparseMatrix& matrix, std::vector& colToRemove) - { - // remove columns through matrix multiplication - SparseMatrix new_matrix(matrix.cols(), matrix.cols() - int(colToRemove.size())); - int iCol = 0; - std::vector> tripletList; - tripletList.reserve(matrix.cols() - int(colToRemove.size())); - - for (int iRow = 0; iRow < matrix.cols(); iRow++) - { - if (std::find(colToRemove.begin(), colToRemove.end(), iRow) == colToRemove.end()) - { - tripletList.push_back(Eigen::Triplet(iRow, iCol, 1)); - iCol++; - } - } - - new_matrix.setFromTriplets(tripletList.begin(), tripletList.end()); - matrix = matrix * new_matrix; - } - - int checkConvergence_getBlocksize(SparseMatrix& m_residuals, Scalar tolerance_L2, std::vector& columnsToDelete) - { - // square roots from sum of squares by column - int BlockSize = m_nev; - Scalar sum, buffer; - - for (int iCol = 0; iCol < m_nev; iCol++) - { - sum = 0; - for (int iRow = 0; iRow < m_n; iRow++) - { - buffer = m_residuals.coeff(iRow, iCol); - sum += buffer * buffer; - } - - if (sqrt(sum) < tolerance_L2) - { - BlockSize--; - columnsToDelete.push_back(iCol); - } - } - return BlockSize; - } - -public: - LOBPCGSolver(const SparseMatrix& A, const SparseMatrix X) : - m_n(A.rows()), - m_nev(X.cols()), - m_info(NOT_COMPUTED), - flag_with_constraints(false), - flag_with_B(false), - flag_with_preconditioner(false), - A(A), - X(X) - { - if (A.rows() != X.rows() || A.rows() != A.cols()) - throw std::invalid_argument("Wrong size"); - - //if (m_n < 5* m_nev) - // throw std::invalid_argument("The problem size is small compared to the block size. Use standard eigensolver"); - } - - void setConstraints(const SparseMatrix& Y) - { - m_Y = Y; - flag_with_constraints = true; - } - - void setB(const SparseMatrix& B) - { - if (B.rows() != A.rows() || B.cols() != A.cols()) - throw std::invalid_argument("Wrong size"); - m_B = B; - flag_with_B = true; - } - - void setPreconditioner(const SparseMatrix& preconditioner) - { - m_preconditioner = preconditioner; - flag_with_preconditioner = true; - } - - void compute(int maxit = 10, Scalar tol_div_n = 1e-7) - { - Scalar tolerance_L2 = tol_div_n * m_n; - int BlockSize; - int max_iter = std::min(m_n, maxit); - - SparseMatrix directions, AX, AR, BX, AD, ADD, DD, BDD, BD, XAD, RAD, DAD, XBD, RBD, BR, sparse_eVecX, sparse_eVecR, sparse_eVecD, inverse_matrix; - Matrix XAR, RAR, XBR, gramA, gramB, eVecX, eVecR, eVecD; - std::vector columnsToDelete; - - if (flag_with_constraints) - { - // Apply the constraints Y to X - applyConstraintsInPlace(X, m_Y, m_B); - } - - // Make initial vectors orthonormal - // implicit BX declaration - if (orthogonalizeInPlace(X, m_B, BX) != SUCCESSFUL) - { - max_iter = 0; - } - - AX = A * X; - // Solve the following NxN eigenvalue problem for all N eigenvalues and -vectors: - // first approximation via a dense problem - Eigen::EigenSolver eigs(Matrix(X.transpose() * AX)); - - if (eigs.info() != SUCCESSFUL) - { - m_info = eigs.info(); - max_iter = 0; - } - else - { - m_evalues = eigs.eigenvalues().real(); - m_evectors = eigs.eigenvectors().real(); - sort_epairs(m_evalues, m_evectors, SMALLEST_ALGE); - sparse_eVecX = m_evectors.sparseView(); - - X = X * sparse_eVecX; - AX = AX * sparse_eVecX; - BX = BX * sparse_eVecX; - } - - for (int iter_num = 0; iter_num < max_iter; iter_num++) - { - m_residuals.resize(m_n, m_nev); - for (int i = 0; i < m_nev; i++) - { - m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i); - } - BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete); - - if (BlockSize == 0) - { - m_info = SUCCESSFUL; - break; - } - - // substitution of the original active mask - if (columnsToDelete.size() > 0) - { - removeColumns(m_residuals, columnsToDelete); - if (iter_num > 0) - { - removeColumns(directions, columnsToDelete); - removeColumns(AD, columnsToDelete); - removeColumns(BD, columnsToDelete); - } - columnsToDelete.clear(); // for next iteration - } - - if (flag_with_preconditioner) - { - // Apply the preconditioner to the residuals - m_residuals = m_preconditioner * m_residuals; - } - - if (flag_with_constraints) - { - // Apply the constraints Y to residuals - applyConstraintsInPlace(m_residuals, m_Y, m_B); - } - - if (orthogonalizeInPlace(m_residuals, m_B, BR) != SUCCESSFUL) - { - break; - } - AR = A * m_residuals; - - // Orthonormalize conjugate directions - if (iter_num > 0) - { - if (orthogonalizeInPlace(directions, m_B, BD, true) != SUCCESSFUL) - { - break; - } - AD = A * directions; - } - - // Perform the Rayleigh Ritz Procedure - XAR = Matrix(X.transpose() * AR); - RAR = Matrix(m_residuals.transpose() * AR); - XBR = Matrix(X.transpose() * BR); - - if (iter_num > 0) - { - XAD = X.transpose() * AD; - RAD = m_residuals.transpose() * AD; - DAD = directions.transpose() * AD; - XBD = X.transpose() * BD; - RBD = m_residuals.transpose() * BD; - - gramA = stack_9_matricies(m_evalues.asDiagonal(), XAR, XAD, XAR.transpose(), RAR, RAD, XAD.transpose(), RAD.transpose(), DAD.transpose()); - gramB = stack_9_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBD, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize), RBD, XBD.transpose(), RBD.transpose(), Matrix::Identity(BlockSize, BlockSize)); - } - else - { - gramA = stack_4_matricies(m_evalues.asDiagonal(), XAR, XAR.transpose(), RAR); - gramB = stack_4_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize)); - } - - //calculate the lowest/largest m eigenpairs; Solve the generalized eigenvalue problem. - DenseSymMatProd Aop(gramA); - DenseCholesky Bop(gramB); - - SymGEigsSolver, - DenseCholesky, GEIGS_CHOLESKY> - geigs(&Aop, &Bop, m_nev, std::min(10, int(gramA.rows()) - 1)); - - geigs.init(); - int nconv = geigs.compute(); - - //Mat evecs; - if (geigs.info() == SUCCESSFUL) - { - m_evalues = geigs.eigenvalues(); - m_evectors = geigs.eigenvectors(); - sort_epairs(m_evalues, m_evectors, SMALLEST_ALGE); - } - else - { - // Problem With General EgenVec - m_info = geigs.info(); - break; - } - - // Compute Ritz vectors - if (iter_num > 0) - { - eVecX = m_evectors.block(0, 0, m_nev, m_nev); - eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev); - eVecD = m_evectors.block(m_nev + BlockSize, 0, BlockSize, m_nev); - - sparse_eVecX = eVecX.sparseView(); - sparse_eVecR = eVecR.sparseView(); - sparse_eVecD = eVecD.sparseView(); - - DD = m_residuals * sparse_eVecR; // new conjugate directions - ADD = AR * sparse_eVecR; - BDD = BR * sparse_eVecR; - - DD = DD + directions * sparse_eVecD; - ADD = ADD + AD * sparse_eVecD; - BDD = BDD + BD * sparse_eVecD; - } - else - { - eVecX = m_evectors.block(0, 0, m_nev, m_nev); - eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev); - - sparse_eVecX = eVecX.sparseView(); - sparse_eVecR = eVecR.sparseView(); - - DD = m_residuals * sparse_eVecR; - ADD = AR * sparse_eVecR; - BDD = BR * sparse_eVecR; - } - - X = X * sparse_eVecX + DD; - AX = AX * sparse_eVecX + ADD; - BX = BX * sparse_eVecX + BDD; - - directions = DD; - AD = ADD; - BD = BDD; - - } // iteration loop - - // calculate last residuals - m_residuals.resize(m_n, m_nev); - for (int i = 0; i < m_nev; i++) - { - m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i); - } - BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete); - - if (BlockSize == 0) - { - m_info = SUCCESSFUL; - } - } // compute - - Vector eigenvalues() - { - return m_evalues; - } - - Matrix eigenvectors() - { - return m_evectors; - } - - Matrix residuals() - { - return Matrix(m_residuals); - } - - int info() { return m_info; } -}; - -} // namespace Spectra - -#endif // LOBPCG_SOLVER +// Written by Anna Araslanova +// Modified by Yixuan Qiu +// License: MIT + +#ifndef SPECTRA_LOBPCG_SOLVER_H +#define SPECTRA_LOBPCG_SOLVER_H + +#include +#include + +#include +#include +#include +#include +#include + +#include "../SymGEigsSolver.h" + +namespace Spectra { + +/// +/// \ingroup EigenSolver +/// + +/// *** METHOD +/// The class represent the LOBPCG algorithm, which was invented by Andrew Knyazev +/// Theoretical background of the procedure can be found in the articles below +/// - Knyazev, A.V., 2001. Toward the optimal preconditioned eigensolver : Locally optimal block preconditioned conjugate gradient method.SIAM journal on scientific computing, 23(2), pp.517 - 541. +/// - Knyazev, A.V., Argentati, M.E., Lashuk, I. and Ovtchinnikov, E.E., 2007. Block locally optimal preconditioned eigenvalue xolvers(BLOPEX) in HYPRE and PETSc.SIAM Journal on Scientific Computing, 29(5), pp.2224 - 2239. +/// +/// *** CONDITIONS OF USE +/// Locally Optimal Block Preconditioned Conjugate Gradient(LOBPCG) is a method for finding the M smallest eigenvalues +/// and eigenvectors of a large symmetric positive definite generalized eigenvalue problem +/// \f$Ax=\lambda Bx,\f$ +/// where \f$A_{NxN}\f$ is a symmetric matrix, \f$B\f$ is symmetric and positive - definite. \f$A and B\f$ are also assumed large and sparse +/// \f$\textit{M}\f$ should be \f$\<< textit{N}\f$ (at least \f$\textit{5M} < \textit{N} \f$) +/// +/// *** ARGUMENTS +/// Eigen::SparseMatrix A; // N*N - Ax = lambda*Bx, lrage and sparse +/// Eigen::SparseMatrix X; // N*M - initial approximations to eigenvectors (random in general case) +/// Spectra::LOBPCGSolver solver(A, X); +/// *Eigen::SparseMatrix B; // N*N - Ax = lambda*Bx, sparse, positive definite +/// solver.setConstraints(B); +/// *Eigen::SparseMatrix Y; // N*K - constraints, already found eigenvectors +/// solver.setB(B); +/// *Eigen::SparseMatrix T; // N*N - preconditioner ~ A^-1 +/// solver.setPreconditioner(T); +/// +/// *** OUTCOMES +/// solver.solve(); // compute eigenpairs // void +/// solver.info(); // state of converjance // int +/// solver.residuals(); // get residuals to evaluate biases // Eigen::Matrix +/// solver.eigenvalues(); // get eigenvalues // Eigen::Matrix +/// solver.eigenvectors(); // get eigenvectors // Eigen::Matrix +/// +/// *** EXAMPLE +/// \code{.cpp} +/// #include +/// +/// // random A +/// Matrix a; +/// a = (Matrix::Random(10, 10).array() > 0.6).cast() * Matrix::Random(10, 10).array() * 5; +/// a = Matrix((a).triangularView()) + Matrix((a).triangularView()).transpose(); +/// for (int i = 0; i < 10; i++) +/// a(i, i) = i + 0.5; +/// std::cout << a << "\n"; +/// Eigen::SparseMatrix A(a.sparseView()); +/// // random X +/// Eigen::Matrix x; +/// x = Matrix::Random(10, 2).array(); +/// Eigen::SparseMatrix X(x.sparseView()); +/// // solve Ax = lambda*x +/// Spectra::LOBPCGSolver solver(A, X); +/// solver.compute(10, 1e-4); // 10 iterations, L2_tolerance = 1e-4*N +/// std::cout << "info\n" << solver.info() << std::endl; +/// std::cout << "eigenvalues\n" << solver.eigenvalues() << std::endl; +/// std::cout << "eigenvectors\n" << solver.eigenvectors() << std::endl; +/// std::cout << "residuals\n" << solver.residuals() << std::endl; +/// \endcode +/// + +template +class LOBPCGSolver +{ +private: + typedef Eigen::Matrix Matrix; + typedef Eigen::Matrix Vector; + + typedef std::complex Complex; + typedef Eigen::Matrix ComplexMatrix; + typedef Eigen::Matrix ComplexVector; + + typedef Eigen::SparseMatrix SparseMatrix; + typedef Eigen::SparseMatrix SparseComplexMatrix; + + const int m_n; // dimension of matrix A + const int m_nev; // number of eigenvalues requested + SparseMatrix A, X; + SparseMatrix m_Y, m_B, m_preconditioner; + bool flag_with_constraints, flag_with_B, flag_with_preconditioner; + +public: + SparseMatrix m_residuals; + Matrix m_evectors; + Vector m_evalues; + int m_info; + +private: + // B-orthonormalize matrix M + int orthogonalizeInPlace(SparseMatrix& M, SparseMatrix& B, + SparseMatrix& true_BM, bool has_true_BM = false) + { + SparseMatrix BM; + + if (has_true_BM == false) + { + if (flag_with_B) + { + BM = B * M; + } + else + { + BM = M; + } + } + else + { + BM = true_BM; + } + + Eigen::SimplicialLDLT chol_MBM(M.transpose() * BM); + + if (chol_MBM.info() != Eigen::Success) + { + // LDLT decomposition fail + m_info = chol_MBM.info(); + return chol_MBM.info(); + } + + SparseComplexMatrix Upper_MBM = chol_MBM.matrixU().template cast(); + ComplexVector D_MBM_vec = chol_MBM.vectorD().template cast(); + + D_MBM_vec = D_MBM_vec.cwiseSqrt(); + + for (int i = 0; i < D_MBM_vec.rows(); i++) + { + D_MBM_vec(i) = Complex(1.0, 0.0) / D_MBM_vec(i); + } + + SparseComplexMatrix D_MBM_mat(D_MBM_vec.asDiagonal()); + + SparseComplexMatrix U_inv(Upper_MBM.rows(), Upper_MBM.cols()); + U_inv.setIdentity(); + Upper_MBM.template triangularView().solveInPlace(U_inv); + + SparseComplexMatrix right_product = U_inv * D_MBM_mat; + M = M * right_product.real(); + if (flag_with_B) + { + true_BM = B * M; + } + else + { + true_BM = M; + } + + return Eigen::Success; + } + + void applyConstraintsInPlace(SparseMatrix& X, SparseMatrix& Y, + SparseMatrix& B) + { + SparseMatrix BY; + if (flag_with_B) + { + BY = B * Y; + } + else + { + BY = Y; + } + + SparseMatrix YBY = Y.transpose() * BY; + SparseMatrix BYX = BY.transpose() * X; + + SparseMatrix YBY_XYX = (Matrix(YBY).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Matrix(BYX))).sparseView(); + X = X - Y * YBY_XYX; + } + + /* + return + 'AB + CD' + */ + Matrix stack_4_matricies(Matrix A, Matrix B, + Matrix C, Matrix D) + { + Matrix result(A.rows() + C.rows(), A.cols() + B.cols()); + result.topLeftCorner(A.rows(), A.cols()) = A; + result.topRightCorner(B.rows(), B.cols()) = B; + result.bottomLeftCorner(C.rows(), C.cols()) = C; + result.bottomRightCorner(D.rows(), D.cols()) = D; + return result; + } + + Matrix stack_9_matricies(Matrix A, Matrix B, Matrix C, + Matrix D, Matrix E, Matrix F, + Matrix G, Matrix H, Matrix I) + { + Matrix result(A.rows() + D.rows() + G.rows(), A.cols() + B.cols() + C.cols()); + result.block(0, 0, A.rows(), A.cols()) = A; + result.block(0, A.cols(), B.rows(), B.cols()) = B; + result.block(0, A.cols() + B.cols(), C.rows(), C.cols()) = C; + result.block(A.rows(), 0, D.rows(), D.cols()) = D; + result.block(A.rows(), A.cols(), E.rows(), E.cols()) = E; + result.block(A.rows(), A.cols() + B.cols(), F.rows(), F.cols()) = F; + result.block(A.rows() + D.rows(), 0, G.rows(), G.cols()) = G; + result.block(A.rows() + D.rows(), A.cols(), H.rows(), H.cols()) = H; + result.block(A.rows() + D.rows(), A.cols() + B.cols(), I.rows(), I.cols()) = I; + + return result; + } + + void sort_epairs(Vector& evalues, Matrix& evectors, SortRule SelectionRule) + { + std::function cmp; + if (SelectionRule == SortRule::SmallestAlge) + cmp = std::less{}; + else + cmp = std::greater{}; + + std::map epairs(cmp); + for (int i = 0; i < m_evectors.cols(); ++i) + epairs.insert(std::make_pair(evalues(i), evectors.col(i))); + + int i = 0; + for (auto& epair : epairs) + { + evectors.col(i) = epair.second; + evalues(i) = epair.first; + i++; + } + } + + void removeColumns(SparseMatrix& matrix, std::vector& colToRemove) + { + // remove columns through matrix multiplication + SparseMatrix new_matrix(matrix.cols(), matrix.cols() - int(colToRemove.size())); + int iCol = 0; + std::vector> tripletList; + tripletList.reserve(matrix.cols() - int(colToRemove.size())); + + for (int iRow = 0; iRow < matrix.cols(); iRow++) + { + if (std::find(colToRemove.begin(), colToRemove.end(), iRow) == colToRemove.end()) + { + tripletList.push_back(Eigen::Triplet(iRow, iCol, 1)); + iCol++; + } + } + + new_matrix.setFromTriplets(tripletList.begin(), tripletList.end()); + matrix = matrix * new_matrix; + } + + int checkConvergence_getBlocksize(SparseMatrix& m_residuals, Scalar tolerance_L2, std::vector& columnsToDelete) + { + // square roots from sum of squares by column + int BlockSize = m_nev; + Scalar sum, buffer; + + for (int iCol = 0; iCol < m_nev; iCol++) + { + sum = 0; + for (int iRow = 0; iRow < m_n; iRow++) + { + buffer = m_residuals.coeff(iRow, iCol); + sum += buffer * buffer; + } + + if (sqrt(sum) < tolerance_L2) + { + BlockSize--; + columnsToDelete.push_back(iCol); + } + } + return BlockSize; + } + +public: + LOBPCGSolver(const SparseMatrix& A, const SparseMatrix X) : + m_n(A.rows()), + m_nev(X.cols()), + A(A), + X(X), + flag_with_constraints(false), + flag_with_B(false), + flag_with_preconditioner(false), + m_info(Eigen::InvalidInput) + { + if (A.rows() != X.rows() || A.rows() != A.cols()) + throw std::invalid_argument("Wrong size"); + + // if (m_n < 5* m_nev) + // throw std::invalid_argument("The problem size is small compared to the block size. Use standard eigensolver"); + } + + void setConstraints(const SparseMatrix& Y) + { + m_Y = Y; + flag_with_constraints = true; + } + + void setB(const SparseMatrix& B) + { + if (B.rows() != A.rows() || B.cols() != A.cols()) + throw std::invalid_argument("Wrong size"); + m_B = B; + flag_with_B = true; + } + + void setPreconditioner(const SparseMatrix& preconditioner) + { + m_preconditioner = preconditioner; + flag_with_preconditioner = true; + } + + void compute(int maxit = 10, Scalar tol_div_n = 1e-7) + { + Scalar tolerance_L2 = tol_div_n * m_n; + int BlockSize; + int max_iter = std::min(m_n, maxit); + + SparseMatrix directions, AX, AR, BX, AD, ADD, DD, BDD, BD, XAD, RAD, DAD, XBD, RBD, BR, sparse_eVecX, sparse_eVecR, sparse_eVecD, inverse_matrix; + Matrix XAR, RAR, XBR, gramA, gramB, eVecX, eVecR, eVecD; + std::vector columnsToDelete; + + if (flag_with_constraints) + { + // Apply the constraints Y to X + applyConstraintsInPlace(X, m_Y, m_B); + } + + // Make initial vectors orthonormal + // implicit BX declaration + if (orthogonalizeInPlace(X, m_B, BX) != Eigen::Success) + { + max_iter = 0; + } + + AX = A * X; + // Solve the following NxN eigenvalue problem for all N eigenvalues and -vectors: + // first approximation via a dense problem + Eigen::EigenSolver eigs(Matrix(X.transpose() * AX)); + + if (eigs.info() != Eigen::Success) + { + m_info = eigs.info(); + max_iter = 0; + } + else + { + m_evalues = eigs.eigenvalues().real(); + m_evectors = eigs.eigenvectors().real(); + sort_epairs(m_evalues, m_evectors, SortRule::SmallestAlge); + sparse_eVecX = m_evectors.sparseView(); + + X = X * sparse_eVecX; + AX = AX * sparse_eVecX; + BX = BX * sparse_eVecX; + } + + for (int iter_num = 0; iter_num < max_iter; iter_num++) + { + m_residuals.resize(m_n, m_nev); + for (int i = 0; i < m_nev; i++) + { + m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i); + } + BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete); + + if (BlockSize == 0) + { + m_info = Eigen::Success; + break; + } + + // substitution of the original active mask + if (columnsToDelete.size() > 0) + { + removeColumns(m_residuals, columnsToDelete); + if (iter_num > 0) + { + removeColumns(directions, columnsToDelete); + removeColumns(AD, columnsToDelete); + removeColumns(BD, columnsToDelete); + } + columnsToDelete.clear(); // for next iteration + } + + if (flag_with_preconditioner) + { + // Apply the preconditioner to the residuals + m_residuals = m_preconditioner * m_residuals; + } + + if (flag_with_constraints) + { + // Apply the constraints Y to residuals + applyConstraintsInPlace(m_residuals, m_Y, m_B); + } + + if (orthogonalizeInPlace(m_residuals, m_B, BR) != Eigen::Success) + { + break; + } + AR = A * m_residuals; + + // Orthonormalize conjugate directions + if (iter_num > 0) + { + if (orthogonalizeInPlace(directions, m_B, BD, true) != Eigen::Success) + { + break; + } + AD = A * directions; + } + + // Perform the Rayleigh Ritz Procedure + XAR = Matrix(X.transpose() * AR); + RAR = Matrix(m_residuals.transpose() * AR); + XBR = Matrix(X.transpose() * BR); + + if (iter_num > 0) + { + XAD = X.transpose() * AD; + RAD = m_residuals.transpose() * AD; + DAD = directions.transpose() * AD; + XBD = X.transpose() * BD; + RBD = m_residuals.transpose() * BD; + + gramA = stack_9_matricies(m_evalues.asDiagonal(), XAR, XAD, XAR.transpose(), RAR, RAD, XAD.transpose(), RAD.transpose(), DAD.transpose()); + gramB = stack_9_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBD, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize), RBD, XBD.transpose(), RBD.transpose(), Matrix::Identity(BlockSize, BlockSize)); + } + else + { + gramA = stack_4_matricies(m_evalues.asDiagonal(), XAR, XAR.transpose(), RAR); + gramB = stack_4_matricies(Matrix::Identity(m_nev, m_nev), XBR, XBR.transpose(), Matrix::Identity(BlockSize, BlockSize)); + } + + // Calculate the lowest/largest m eigenpairs; Solve the generalized eigenvalue problem. + DenseSymMatProd Aop(gramA); + DenseCholesky Bop(gramB); + + SymGEigsSolver, DenseCholesky, GEigsMode::Cholesky> + geigs(Aop, Bop, m_nev, (std::min)(10, int(gramA.rows()) - 1)); + + geigs.init(); + geigs.compute(SortRule::SmallestAlge); + + // Mat evecs + if (geigs.info() == CompInfo::Successful) + { + m_evalues = geigs.eigenvalues(); + m_evectors = geigs.eigenvectors(); + sort_epairs(m_evalues, m_evectors, SortRule::SmallestAlge); + } + else + { + // Problem With General EgenVec + m_info = Eigen::NoConvergence; + break; + } + + // Compute Ritz vectors + if (iter_num > 0) + { + eVecX = m_evectors.block(0, 0, m_nev, m_nev); + eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev); + eVecD = m_evectors.block(m_nev + BlockSize, 0, BlockSize, m_nev); + + sparse_eVecX = eVecX.sparseView(); + sparse_eVecR = eVecR.sparseView(); + sparse_eVecD = eVecD.sparseView(); + + DD = m_residuals * sparse_eVecR; // new conjugate directions + ADD = AR * sparse_eVecR; + BDD = BR * sparse_eVecR; + + DD = DD + directions * sparse_eVecD; + ADD = ADD + AD * sparse_eVecD; + BDD = BDD + BD * sparse_eVecD; + } + else + { + eVecX = m_evectors.block(0, 0, m_nev, m_nev); + eVecR = m_evectors.block(m_nev, 0, BlockSize, m_nev); + + sparse_eVecX = eVecX.sparseView(); + sparse_eVecR = eVecR.sparseView(); + + DD = m_residuals * sparse_eVecR; + ADD = AR * sparse_eVecR; + BDD = BR * sparse_eVecR; + } + + X = X * sparse_eVecX + DD; + AX = AX * sparse_eVecX + ADD; + BX = BX * sparse_eVecX + BDD; + + directions = DD; + AD = ADD; + BD = BDD; + + } // iteration loop + + // calculate last residuals + m_residuals.resize(m_n, m_nev); + for (int i = 0; i < m_nev; i++) + { + m_residuals.col(i) = AX.col(i) - m_evalues(i) * BX.col(i); + } + BlockSize = checkConvergence_getBlocksize(m_residuals, tolerance_L2, columnsToDelete); + + if (BlockSize == 0) + { + m_info = Eigen::Success; + } + } // compute + + Vector eigenvalues() + { + return m_evalues; + } + + Matrix eigenvectors() + { + return m_evectors; + } + + Matrix residuals() + { + return Matrix(m_residuals); + } + + int info() { return m_info; } +}; + +} // namespace Spectra + +#endif // SPECTRA_LOBPCG_SOLVER_H diff --git a/gtsam/3rdparty/Spectra/contrib/PartialSVDSolver.h b/gtsam/3rdparty/Spectra/contrib/PartialSVDSolver.h index 2fab26b97a..1ecf475078 100644 --- a/gtsam/3rdparty/Spectra/contrib/PartialSVDSolver.h +++ b/gtsam/3rdparty/Spectra/contrib/PartialSVDSolver.h @@ -1,11 +1,11 @@ -// Copyright (C) 2018 Yixuan Qiu +// Copyright (C) 2018-2025 Yixuan Qiu // // This Source Code Form is subject to the terms of the Mozilla // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at https://mozilla.org/MPL/2.0/. -#ifndef PARTIAL_SVD_SOLVER_H -#define PARTIAL_SVD_SOLVER_H +#ifndef SPECTRA_PARTIAL_SVD_SOLVER_H +#define SPECTRA_PARTIAL_SVD_SOLVER_H #include #include "../SymEigsSolver.h" @@ -13,15 +13,21 @@ namespace Spectra { // Abstract class for matrix operation -template +template class SVDMatOp { public: - virtual int rows() const = 0; - virtual int cols() const = 0; + using Scalar = Scalar_; + +private: + using Index = Eigen::Index; + +public: + virtual Index rows() const = 0; + virtual Index cols() const = 0; // y_out = A' * A * x_in or y_out = A * A' * x_in - virtual void perform_op(const Scalar* x_in, Scalar* y_out) = 0; + virtual void perform_op(const Scalar* x_in, Scalar* y_out) const = 0; virtual ~SVDMatOp() {} }; @@ -33,29 +39,30 @@ template class SVDTallMatOp : public SVDMatOp { private: - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; - const int m_dim; - Vector m_cache; + const Index m_dim; + mutable Vector m_cache; public: // Constructor SVDTallMatOp(ConstGenericMatrix& mat) : m_mat(mat), - m_dim(std::min(mat.rows(), mat.cols())), + m_dim((std::min)(mat.rows(), mat.cols())), m_cache(mat.rows()) {} // These are the rows and columns of A' * A - int rows() const { return m_dim; } - int cols() const { return m_dim; } + Index rows() const override { return m_dim; } + Index cols() const override { return m_dim; } // y_out = A' * A * x_in - void perform_op(const Scalar* x_in, Scalar* y_out) + void perform_op(const Scalar* x_in, Scalar* y_out) const override { MapConstVec x(x_in, m_mat.cols()); MapVec y(y_out, m_mat.cols()); @@ -71,29 +78,30 @@ template class SVDWideMatOp : public SVDMatOp { private: - typedef Eigen::Matrix Vector; - typedef Eigen::Map MapConstVec; - typedef Eigen::Map MapVec; - typedef const Eigen::Ref ConstGenericMatrix; + using Index = Eigen::Index; + using Vector = Eigen::Matrix; + using MapConstVec = Eigen::Map; + using MapVec = Eigen::Map; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; - const int m_dim; - Vector m_cache; + const Index m_dim; + mutable Vector m_cache; public: // Constructor SVDWideMatOp(ConstGenericMatrix& mat) : m_mat(mat), - m_dim(std::min(mat.rows(), mat.cols())), + m_dim((std::min)(mat.rows(), mat.cols())), m_cache(mat.cols()) {} // These are the rows and columns of A * A' - int rows() const { return m_dim; } - int cols() const { return m_dim; } + Index rows() const override { return m_dim; } + Index cols() const override { return m_dim; } // y_out = A * A' * x_in - void perform_op(const Scalar* x_in, Scalar* y_out) + void perform_op(const Scalar* x_in, Scalar* y_out) const override { MapConstVec x(x_in, m_mat.rows()); MapVec y(y_out, m_mat.rows()); @@ -104,26 +112,27 @@ class SVDWideMatOp : public SVDMatOp // Partial SVD solver // MatrixType is either Eigen::Matrix or Eigen::SparseMatrix -template > +template > class PartialSVDSolver { private: - typedef Eigen::Matrix Matrix; - typedef Eigen::Matrix Vector; - typedef const Eigen::Ref ConstGenericMatrix; + using Scalar = typename MatrixType::Scalar; + using Index = Eigen::Index; + using Matrix = Eigen::Matrix; + using Vector = Eigen::Matrix; + using ConstGenericMatrix = const Eigen::Ref; ConstGenericMatrix m_mat; - const int m_m; - const int m_n; + const Index m_m; + const Index m_n; SVDMatOp* m_op; - SymEigsSolver >* m_eigs; - int m_nconv; + SymEigsSolver>* m_eigs; + Index m_nconv; Matrix m_evecs; public: // Constructor - PartialSVDSolver(ConstGenericMatrix& mat, int ncomp, int ncv) : + PartialSVDSolver(ConstGenericMatrix& mat, Index ncomp, Index ncv) : m_mat(mat), m_m(mat.rows()), m_n(mat.cols()), m_evecs(0, 0) { // Determine the matrix type, tall or wide @@ -137,7 +146,7 @@ class PartialSVDSolver } // Solver object - m_eigs = new SymEigsSolver >(m_op, ncomp, ncv); + m_eigs = new SymEigsSolver>(*m_op, ncomp, ncv); } // Destructor @@ -148,10 +157,10 @@ class PartialSVDSolver } // Computation - int compute(int maxit = 1000, Scalar tol = 1e-10) + Index compute(Index maxit = 1000, Scalar tol = 1e-10) { m_eigs->init(); - m_nconv = m_eigs->compute(maxit, tol); + m_nconv = m_eigs->compute(SortRule::LargestAlge, maxit, tol); return m_nconv; } @@ -165,13 +174,13 @@ class PartialSVDSolver } // The converged left singular vectors - Matrix matrix_U(int nu) + Matrix matrix_U(Index nu) { if (m_evecs.cols() < 1) { m_evecs = m_eigs->eigenvectors(); } - nu = std::min(nu, m_nconv); + nu = (std::min)(nu, m_nconv); if (m_m <= m_n) { return m_evecs.leftCols(nu); @@ -181,13 +190,13 @@ class PartialSVDSolver } // The converged right singular vectors - Matrix matrix_V(int nv) + Matrix matrix_V(Index nv) { if (m_evecs.cols() < 1) { m_evecs = m_eigs->eigenvectors(); } - nv = std::min(nv, m_nconv); + nv = (std::min)(nv, m_nconv); if (m_m > m_n) { return m_evecs.leftCols(nv); @@ -199,4 +208,4 @@ class PartialSVDSolver } // namespace Spectra -#endif // PARTIAL_SVD_SOLVER_H +#endif // SPECTRA_PARTIAL_SVD_SOLVER_H From 274acb9e8294f667ef20bfc140360fd6931e43ba Mon Sep 17 00:00:00 2001 From: Martin Valgur Date: Mon, 9 Dec 2024 17:12:49 +0200 Subject: [PATCH 2/5] Update ShonanAveraging.cpp to use Spectra v1.1.0 --- gtsam/sfm/ShonanAveraging.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 6c9f91e903..e8bb41fc14 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -570,6 +570,8 @@ static bool PowerMinimumEigenValue( * nontrivial function, perform_op(x,y), that computes and returns the product * y = (A + sigma*I) x */ struct MatrixProdFunctor { + using Scalar = double; + // Const reference to an externally-held matrix whose minimum-eigenvalue we // want to compute const Sparse &A_; @@ -630,13 +632,13 @@ static bool SparseMinimumEigenValue( Eigen::Index numLanczosVectors = 20) { // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos MatrixProdFunctor lmOperator(A); - Spectra::SymEigsSolver - lmEigenValueSolver(&lmOperator, 1, std::min(numLanczosVectors, A.rows())); + Spectra::SymEigsSolver lmEigenValueSolver( + lmOperator, 1, std::min(numLanczosVectors, A.rows())); lmEigenValueSolver.init(); - const int lmConverged = lmEigenValueSolver.compute( - maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); + const int lmConverged = + lmEigenValueSolver.compute(Spectra::SortRule::LargestMagn, maxIterations, + 1e-4, Spectra::SortRule::LargestMagn); // Check convergence and bail out if necessary if (lmConverged != 1) return false; @@ -664,10 +666,8 @@ static bool SparseMinimumEigenValue( MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue); - Spectra::SymEigsSolver - minEigenValueSolver(&minShiftedOperator, 1, - std::min(numLanczosVectors, A.rows())); + Spectra::SymEigsSolver minEigenValueSolver( + minShiftedOperator, 1, std::min(numLanczosVectors, A.rows())); // If S is a critical point of F, then S^T is also in the null space of S - // Lambda(S) (cf. Lemma 6 of the tech report), and therefore its rows are @@ -695,8 +695,9 @@ static bool SparseMinimumEigenValue( // order to be able to estimate the smallest eigenvalue within an *absolute* // tolerance of 'minEigenvalueNonnegativityTolerance' const int minConverged = minEigenValueSolver.compute( - maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue, - Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); + Spectra::SortRule::LargestMagn, maxIterations, + minEigenvalueNonnegativityTolerance / lmEigenValue, + Spectra::SortRule::LargestMagn); if (minConverged != 1) return false; From 6a42d8a1b24216513c642dffa3c4a54477e02f94 Mon Sep 17 00:00:00 2001 From: Martin Valgur Date: Mon, 9 Dec 2024 16:23:40 +0200 Subject: [PATCH 3/5] Fix wrapping of computed angles in test_constructorBetweenFactorPose2s --- python/gtsam/tests/test_ShonanAveraging.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_ShonanAveraging.py b/python/gtsam/tests/test_ShonanAveraging.py index fc09437721..5c36ad361b 100644 --- a/python/gtsam/tests/test_ShonanAveraging.py +++ b/python/gtsam/tests/test_ShonanAveraging.py @@ -192,9 +192,8 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - # map all angles to [0,360) - thetas_deg = thetas_deg % 360 - thetas_deg -= thetas_deg[0] + # map all angles to [-180,180) + thetas_deg = (thetas_deg - thetas_deg[0] + 180) % 360 - 180 expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1) From b2a351c8de068dc834b1aa2d293ee8807df6ba06 Mon Sep 17 00:00:00 2001 From: Martin Valgur Date: Mon, 9 Dec 2024 17:18:31 +0200 Subject: [PATCH 4/5] ShonanAveraging.cpp: pass explicit template args for GCC 7 --- gtsam/sfm/ShonanAveraging.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index e8bb41fc14..7d68c7d29f 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -632,7 +632,7 @@ static bool SparseMinimumEigenValue( Eigen::Index numLanczosVectors = 20) { // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos MatrixProdFunctor lmOperator(A); - Spectra::SymEigsSolver lmEigenValueSolver( + Spectra::SymEigsSolver lmEigenValueSolver( lmOperator, 1, std::min(numLanczosVectors, A.rows())); lmEigenValueSolver.init(); @@ -666,7 +666,7 @@ static bool SparseMinimumEigenValue( MatrixProdFunctor minShiftedOperator(A, -2 * lmEigenValue); - Spectra::SymEigsSolver minEigenValueSolver( + Spectra::SymEigsSolver minEigenValueSolver( minShiftedOperator, 1, std::min(numLanczosVectors, A.rows())); // If S is a critical point of F, then S^T is also in the null space of S - From 85831851539236ebb82c31795221ce38f482faaf Mon Sep 17 00:00:00 2001 From: Martin Valgur Date: Tue, 10 Dec 2024 20:37:15 +0200 Subject: [PATCH 5/5] Fix wrapping of computed angles in test_backwards_compatibility.py --- python/gtsam/tests/test_backwards_compatibility.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/python/gtsam/tests/test_backwards_compatibility.py b/python/gtsam/tests/test_backwards_compatibility.py index ca96cdf579..c64be37a7d 100644 --- a/python/gtsam/tests/test_backwards_compatibility.py +++ b/python/gtsam/tests/test_backwards_compatibility.py @@ -472,9 +472,8 @@ def test_constructorBetweenFactorPose2s(self) -> None: wRi_list = [result_values.atRot2(i) for i in range(num_images)] thetas_deg = np.array([wRi.degrees() for wRi in wRi_list]) - # map all angles to [0,360) - thetas_deg = thetas_deg % 360 - thetas_deg -= thetas_deg[0] + # map all angles to [-180,180) + thetas_deg = (thetas_deg - thetas_deg[0] + 180) % 360 - 180 expected_thetas_deg = np.array([0.0, 90.0, 0.0]) np.testing.assert_allclose(thetas_deg, expected_thetas_deg, atol=0.1)