SymShiftInvert.h
Go to the documentation of this file.
1 // Copyright (C) 2020-2025 Yixuan Qiu <yixuan.qiu@cos.name>
2 //
3 // This Source Code Form is subject to the terms of the Mozilla
4 // Public License v. 2.0. If a copy of the MPL was not distributed
5 // with this file, You can obtain one at https://mozilla.org/MPL/2.0/.
6 
7 #ifndef SPECTRA_SYM_SHIFT_INVERT_H
8 #define SPECTRA_SYM_SHIFT_INVERT_H
9 
10 #include <Eigen/Core>
11 #include <Eigen/SparseCore>
12 #include <Eigen/SparseLU>
13 #include <stdexcept>
14 #include <type_traits> // std::conditional, std::is_same
15 
16 #include "../LinAlg/BKLDLT.h"
17 #include "../Util/CompInfo.h"
18 
19 namespace Spectra {
20 
22 
23 // Compute and factorize A-sigma*B without unnecessary copying
24 // Default case: A is sparse, B is sparse
25 template <bool AIsSparse, bool BIsSparse, int UploA, int UploB>
26 class SymShiftInvertHelper
27 {
28 public:
29  template <typename Scalar, typename Fac, typename ArgA, typename ArgB>
30  static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma)
31  {
32  using SpMat = typename ArgA::PlainObject;
33  SpMat matA = A.template selfadjointView<UploA>();
34  SpMat matB = B.template selfadjointView<UploB>();
35  SpMat mat = matA - sigma * matB;
36  // SparseLU solver
37  fac.isSymmetric(true);
38  fac.compute(mat);
39  // Return true if successful
40  return fac.info() == Eigen::Success;
41  }
42 };
43 
44 // A is dense, B is dense or sparse
45 template <bool BIsSparse, int UploA, int UploB>
46 class SymShiftInvertHelper<false, BIsSparse, UploA, UploB>
47 {
48 public:
49  template <typename Scalar, typename Fac, typename ArgA, typename ArgB>
50  static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma)
51  {
52  using Matrix = typename ArgA::PlainObject;
53  // Make a copy of the <UploA> triangular part of A
54  Matrix mat(A.rows(), A.cols());
55  mat.template triangularView<UploA>() = A;
56  // Update <UploA> triangular part of mat
57  if (UploA == UploB)
58  mat -= (B * sigma).template triangularView<UploA>();
59  else
60  mat -= (B * sigma).template triangularView<UploB>().transpose();
61  // BKLDLT solver
62  fac.compute(mat, UploA);
63  // Return true if successful
64  return fac.info() == CompInfo::Successful;
65  }
66 };
67 
68 // A is sparse, B is dense
69 template <int UploA, int UploB>
70 class SymShiftInvertHelper<true, false, UploA, UploB>
71 {
72 public:
73  template <typename Scalar, typename Fac, typename ArgA, typename ArgB>
74  static bool factorize(Fac& fac, const ArgA& A, const ArgB& B, const Scalar& sigma)
75  {
76  using Matrix = typename ArgB::PlainObject;
77  // Construct the <UploB> triangular part of -sigma*B
78  Matrix mat(B.rows(), B.cols());
79  mat.template triangularView<UploB>() = -sigma * B;
80  // Update <UploB> triangular part of mat
81  if (UploA == UploB)
82  mat += A.template triangularView<UploB>();
83  else
84  mat += A.template triangularView<UploA>().transpose();
85  // BKLDLT solver
86  fac.compute(mat, UploB);
87  // Return true if successful
88  return fac.info() == CompInfo::Successful;
89  }
90 };
91 
93 
123 template <typename Scalar_, typename TypeA = Eigen::Sparse, typename TypeB = Eigen::Sparse,
124  int UploA = Eigen::Lower, int UploB = Eigen::Lower,
125  int FlagsA = Eigen::ColMajor, int FlagsB = Eigen::ColMajor,
126  typename StorageIndexA = int, typename StorageIndexB = int>
128 {
129 public:
133  using Scalar = Scalar_;
134 
135 private:
137 
138  // Hypothetical type of the A matrix, either dense or sparse
141  // Whether A is sparse
142  using ASparse = std::is_same<TypeA, Eigen::Sparse>;
143  // Actual type of the A matrix
145 
146  // Hypothetical type of the B matrix, either dense or sparse
149  // Whether B is sparse
150  using BSparse = std::is_same<TypeB, Eigen::Sparse>;
151  // Actual type of the B matrix
153 
157 
158  // The type of A-sigma*B if one of A and B is dense
159  // DenseType = if (A is dense) MatrixA else MatrixB
161  // The type of A-sigma*B
162  // If both A and B are sparse, the result is MatrixA; otherwise the result is DenseType
164 
165  // If both A and B are sparse, then the result A-sigma*B is sparse, so we use
166  // sparseLU for factorization; otherwise A-sigma*B is dense, and we use BKLDLT
167  using FacType = typename std::conditional<
171 
174 
177  const Index m_n;
179 
180 public:
190  template <typename DerivedA, typename DerivedB>
192  m_matA(A.derived()), m_matB(B.derived()), m_n(A.rows())
193  {
194  static_assert(
195  static_cast<int>(DerivedA::PlainObject::IsRowMajor) == static_cast<int>(MatrixA::IsRowMajor),
196  "SymShiftInvert: the \"FlagsA\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)");
197 
198  static_assert(
199  static_cast<int>(DerivedB::PlainObject::IsRowMajor) == static_cast<int>(MatrixB::IsRowMajor),
200  "SymShiftInvert: the \"FlagsB\" template parameter does not match the input matrix (Eigen::ColMajor/Eigen::RowMajor)");
201 
202  if (m_n != A.cols() || m_n != B.rows() || m_n != B.cols())
203  throw std::invalid_argument("SymShiftInvert: A and B must be square matrices of the same size");
204  }
205 
209  Index rows() const { return m_n; }
213  Index cols() const { return m_n; }
214 
218  void set_shift(const Scalar& sigma)
219  {
220  constexpr bool AIsSparse = ASparse::value;
221  constexpr bool BIsSparse = BSparse::value;
222  using Helper = SymShiftInvertHelper<AIsSparse, BIsSparse, UploA, UploB>;
223  const bool success = Helper::factorize(m_solver, m_matA, m_matB, sigma);
224  if (!success)
225  throw std::invalid_argument("SymShiftInvert: factorization failed with the given shift");
226  }
227 
234  // y_out = inv(A - sigma * B) * x_in
235  void perform_op(const Scalar* x_in, Scalar* y_out) const
236  {
237  MapConstVec x(x_in, m_n);
238  MapVec y(y_out, m_n);
239  y.noalias() = m_solver.solve(x);
240  }
241 };
242 
243 } // namespace Spectra
244 
245 #endif // SPECTRA_SYM_SHIFT_INVERT_H
matA
MatrixXf matA(2, 2)
gtsam.examples.DogLegOptimizerExample.int
int
Definition: DogLegOptimizerExample.py:111
Spectra::SymShiftInvert::cols
Index cols() const
Definition: SymShiftInvert.h:213
Eigen::SparseMatrix< double >
Spectra::SymShiftInvert::m_matA
ConstGenericMatrixA m_matA
Definition: SymShiftInvert.h:175
matB
MatrixXf matB(2, 2)
gtsam.examples.DogLegOptimizerExample.type
type
Definition: DogLegOptimizerExample.py:111
Spectra::CompInfo::Successful
@ Successful
Computation was successful.
Spectra::SymShiftInvert< double >::DenseType
typename std::conditional< ASparse::value, MatrixB, MatrixA >::type DenseType
Definition: SymShiftInvert.h:160
Eigen::EigenBase
Definition: EigenBase.h:29
Spectra::SymShiftInvert< double >::MatrixA
typename std::conditional< ASparse::value, SparseTypeA, DenseTypeA >::type MatrixA
Definition: SymShiftInvert.h:144
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
B
Definition: test_numpy_dtypes.cpp:301
Eigen::Success
@ Success
Definition: Constants.h:442
mat
MatrixXf mat
Definition: Tutorial_AdvancedInitialization_CommaTemporary.cpp:1
Spectra::SymShiftInvert::perform_op
void perform_op(const Scalar *x_in, Scalar *y_out) const
Definition: SymShiftInvert.h:235
sampling::sigma
static const double sigma
Definition: testGaussianBayesNet.cpp:170
Spectra::SymShiftInvert< double >::ResType
typename std::conditional< ASparse::value &&BSparse::value, MatrixA, DenseType >::type ResType
Definition: SymShiftInvert.h:163
ceres::Matrix
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
Definition: gtsam/3rdparty/ceres/eigen.h:42
Spectra::SymShiftInvert< double >::Index
Eigen::Index Index
Definition: SymShiftInvert.h:136
Spectra::SymShiftInvert::set_shift
void set_shift(const Scalar &sigma)
Definition: SymShiftInvert.h:218
A
Definition: test_numpy_dtypes.cpp:300
Spectra::SymShiftInvert< double >::BSparse
std::is_same< Eigen::Sparse, Eigen::Sparse > BSparse
Definition: SymShiftInvert.h:150
Spectra::SymShiftInvert::rows
Index rows() const
Definition: SymShiftInvert.h:209
Eigen::Lower
@ Lower
Definition: Constants.h:209
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Spectra::SymShiftInvert< double >::Scalar
double Scalar
Definition: SymShiftInvert.h:133
Spectra::SymShiftInvert::m_solver
FacType m_solver
Definition: SymShiftInvert.h:178
y
Scalar * y
Definition: level1_cplx_impl.h:124
Eigen::Ref< const MatrixA >
Spectra::SymShiftInvert< double >::FacType
typename std::conditional< ASparse::value &&BSparse::value, Eigen::SparseLU< ResType >, BKLDLT< Scalar > >::type FacType
Definition: SymShiftInvert.h:170
gtsam::Sparse
Eigen::SparseMatrix< double > Sparse
Definition: AcceleratedPowerMethod.h:26
Spectra::SymShiftInvert::m_matB
ConstGenericMatrixB m_matB
Definition: SymShiftInvert.h:176
Spectra::SymShiftInvert::SymShiftInvert
SymShiftInvert(const Eigen::EigenBase< DerivedA > &A, const Eigen::EigenBase< DerivedB > &B)
Definition: SymShiftInvert.h:191
Spectra
Definition: LOBPCGSolver.h:19
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
Eigen::SparseLU
Sparse supernodal LU factorization for general matrices.
Definition: SparseLU.h:17
Spectra::SymShiftInvert
Definition: SymShiftInvert.h:127
Eigen::ColMajor
@ ColMajor
Definition: Constants.h:319
Spectra::SymShiftInvert< double >::ASparse
std::is_same< Eigen::Sparse, Eigen::Sparse > ASparse
Definition: SymShiftInvert.h:142
test_callbacks.value
value
Definition: test_callbacks.py:162
Spectra::BKLDLT
Definition: BKLDLT.h:65
Spectra::SymShiftInvert< double >::MatrixB
typename std::conditional< BSparse::value, SparseTypeB, DenseTypeB >::type MatrixB
Definition: SymShiftInvert.h:152
Scalar
SCALAR Scalar
Definition: bench_gemm.cpp:46
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
Spectra::SymShiftInvert::m_n
const Index m_n
Definition: SymShiftInvert.h:177


gtsam
Author(s):
autogenerated on Thu Apr 10 2025 03:04:25