BKLDLT.h
Go to the documentation of this file.
1 // Copyright (C) 2019 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 BK_LDLT_H
8 #define BK_LDLT_H
9 
10 #include <Eigen/Core>
11 #include <vector>
12 #include <stdexcept>
13 
14 #include "../Util/CompInfo.h"
15 
16 namespace Spectra {
17 
18 // Bunch-Kaufman LDLT decomposition
19 // References:
20 // 1. Bunch, J. R., & Kaufman, L. (1977). Some stable methods for calculating inertia and solving symmetric linear systems.
21 // Mathematics of computation, 31(137), 163-179.
22 // 2. Golub, G. H., & Van Loan, C. F. (2012). Matrix computations (Vol. 3). JHU press. Section 4.4.
23 // 3. Bunch-Parlett diagonal pivoting <http://oz.nthu.edu.tw/~d947207/Chap13_GE3.ppt>
24 // 4. Ashcraft, C., Grimes, R. G., & Lewis, J. G. (1998). Accurate symmetric indefinite linear equation solvers.
25 // SIAM Journal on Matrix Analysis and Applications, 20(2), 513-561.
26 template <typename Scalar = double>
27 class BKLDLT
28 {
29 private:
35 
41 
42  Index m_n;
43  Vector m_data; // storage for a lower-triangular matrix
44  std::vector<Scalar*> m_colptr; // pointers to columns
45  IntVector m_perm; // [-2, -1, 3, 1, 4, 5]: 0 <-> 2, 1 <-> 1, 2 <-> 3, 3 <-> 1, 4 <-> 4, 5 <-> 5
46  std::vector<std::pair<Index, Index> > m_permc; // compressed version of m_perm: [(0, 2), (2, 3), (3, 1)]
47 
48  bool m_computed;
49  int m_info;
50 
51  // Access to elements
52  // Pointer to the k-th column
53  Scalar* col_pointer(Index k) { return m_colptr[k]; }
54  // A[i, j] -> m_colptr[j][i - j], i >= j
55  Scalar& coeff(Index i, Index j) { return m_colptr[j][i - j]; }
56  const Scalar& coeff(Index i, Index j) const { return m_colptr[j][i - j]; }
57  // A[i, i] -> m_colptr[i][0]
58  Scalar& diag_coeff(Index i) { return m_colptr[i][0]; }
59  const Scalar& diag_coeff(Index i) const { return m_colptr[i][0]; }
60 
61  // Compute column pointers
63  {
64  m_colptr.clear();
65  m_colptr.reserve(m_n);
66  Scalar* head = m_data.data();
67 
68  for (Index i = 0; i < m_n; i++)
69  {
70  m_colptr.push_back(head);
71  head += (m_n - i);
72  }
73  }
74 
75  // Copy mat - shift * I to m_data
76  void copy_data(ConstGenericMatrix& mat, int uplo, const Scalar& shift)
77  {
78  if (uplo == Eigen::Lower)
79  {
80  for (Index j = 0; j < m_n; j++)
81  {
82  const Scalar* begin = &mat.coeffRef(j, j);
83  const Index len = m_n - j;
84  std::copy(begin, begin + len, col_pointer(j));
85  diag_coeff(j) -= shift;
86  }
87  }
88  else
89  {
90  Scalar* dest = m_data.data();
91  for (Index i = 0; i < m_n; i++)
92  {
93  for (Index j = i; j < m_n; j++, dest++)
94  {
95  *dest = mat.coeff(i, j);
96  }
97  diag_coeff(i) -= shift;
98  }
99  }
100  }
101 
102  // Compute compressed permutations
104  {
105  for (Index i = 0; i < m_n; i++)
106  {
107  // Recover the permutation action
108  const Index perm = (m_perm[i] >= 0) ? (m_perm[i]) : (-m_perm[i] - 1);
109  if (perm != i)
110  m_permc.push_back(std::make_pair(i, perm));
111  }
112  }
113 
114  // Working on the A[k:end, k:end] submatrix
115  // Exchange k <-> r
116  // Assume r >= k
117  void pivoting_1x1(Index k, Index r)
118  {
119  // No permutation
120  if (k == r)
121  {
122  m_perm[k] = r;
123  return;
124  }
125 
126  // A[k, k] <-> A[r, r]
128 
129  // A[(r+1):end, k] <-> A[(r+1):end, r]
130  std::swap_ranges(&coeff(r + 1, k), col_pointer(k + 1), &coeff(r + 1, r));
131 
132  // A[(k+1):(r-1), k] <-> A[r, (k+1):(r-1)]
133  Scalar* src = &coeff(k + 1, k);
134  for (Index j = k + 1; j < r; j++, src++)
135  {
136  std::swap(*src, coeff(r, j));
137  }
138 
139  m_perm[k] = r;
140  }
141 
142  // Working on the A[k:end, k:end] submatrix
143  // Exchange [k+1, k] <-> [r, p]
144  // Assume p >= k, r >= k+1
145  void pivoting_2x2(Index k, Index r, Index p)
146  {
147  pivoting_1x1(k, p);
148  pivoting_1x1(k + 1, r);
149 
150  // A[k+1, k] <-> A[r, k]
151  std::swap(coeff(k + 1, k), coeff(r, k));
152 
153  // Use negative signs to indicate a 2x2 block
154  // Also minus one to distinguish a negative zero from a positive zero
155  m_perm[k] = -m_perm[k] - 1;
156  m_perm[k + 1] = -m_perm[k + 1] - 1;
157  }
158 
159  // A[r1, c1:c2] <-> A[r2, c1:c2]
160  // Assume r2 >= r1 > c2 >= c1
161  void interchange_rows(Index r1, Index r2, Index c1, Index c2)
162  {
163  if (r1 == r2)
164  return;
165 
166  for (Index j = c1; j <= c2; j++)
167  {
168  std::swap(coeff(r1, j), coeff(r2, j));
169  }
170  }
171 
172  // lambda = |A[r, k]| = max{|A[k+1, k]|, ..., |A[end, k]|}
173  // Largest (in magnitude) off-diagonal element in the first column of the current reduced matrix
174  // r is the row index
175  // Assume k < end
176  Scalar find_lambda(Index k, Index& r)
177  {
178  using std::abs;
179 
180  const Scalar* head = col_pointer(k); // => A[k, k]
181  const Scalar* end = col_pointer(k + 1);
182  // Start with r=k+1, lambda=A[k+1, k]
183  r = k + 1;
184  Scalar lambda = abs(head[1]);
185  // Scan remaining elements
186  for (const Scalar* ptr = head + 2; ptr < end; ptr++)
187  {
188  const Scalar abs_elem = abs(*ptr);
189  if (lambda < abs_elem)
190  {
191  lambda = abs_elem;
192  r = k + (ptr - head);
193  }
194  }
195 
196  return lambda;
197  }
198 
199  // sigma = |A[p, r]| = max {|A[k, r]|, ..., |A[end, r]|} \ {A[r, r]}
200  // Largest (in magnitude) off-diagonal element in the r-th column of the current reduced matrix
201  // p is the row index
202  // Assume k < r < end
203  Scalar find_sigma(Index k, Index r, Index& p)
204  {
205  using std::abs;
206 
207  // First search A[r+1, r], ..., A[end, r], which has the same task as find_lambda()
208  // If r == end, we skip this search
209  Scalar sigma = Scalar(-1);
210  if (r < m_n - 1)
211  sigma = find_lambda(r, p);
212 
213  // Then search A[k, r], ..., A[r-1, r], which maps to A[r, k], ..., A[r, r-1]
214  for (Index j = k; j < r; j++)
215  {
216  const Scalar abs_elem = abs(coeff(r, j));
217  if (sigma < abs_elem)
218  {
219  sigma = abs_elem;
220  p = j;
221  }
222  }
223 
224  return sigma;
225  }
226 
227  // Generate permutations and apply to A
228  // Return true if the resulting pivoting is 1x1, and false if 2x2
229  bool permutate_mat(Index k, const Scalar& alpha)
230  {
231  using std::abs;
232 
233  Index r = k, p = k;
234  const Scalar lambda = find_lambda(k, r);
235 
236  // If lambda=0, no need to interchange
237  if (lambda > Scalar(0))
238  {
239  const Scalar abs_akk = abs(diag_coeff(k));
240  // If |A[k, k]| >= alpha * lambda, no need to interchange
241  if (abs_akk < alpha * lambda)
242  {
243  const Scalar sigma = find_sigma(k, r, p);
244 
245  // If sigma * |A[k, k]| >= alpha * lambda^2, no need to interchange
246  if (sigma * abs_akk < alpha * lambda * lambda)
247  {
248  if (abs_akk >= alpha * sigma)
249  {
250  // Permutation on A
251  pivoting_1x1(k, r);
252 
253  // Permutation on L
254  interchange_rows(k, r, 0, k - 1);
255  return true;
256  }
257  else
258  {
259  // There are two versions of permutation here
260  // 1. A[k+1, k] <-> A[r, k]
261  // 2. A[k+1, k] <-> A[r, p], where p >= k and r >= k+1
262  //
263  // Version 1 and 2 are used by Ref[1] and Ref[2], respectively
264 
265  // Version 1 implementation
266  p = k;
267 
268  // Version 2 implementation
269  // [r, p] and [p, r] are symmetric, but we need to make sure
270  // p >= k and r >= k+1, so it is safe to always make r > p
271  // One exception is when min{r,p} == k+1, in which case we make
272  // r = k+1, so that only one permutation needs to be performed
273  /* const Index rp_min = std::min(r, p);
274  const Index rp_max = std::max(r, p);
275  if(rp_min == k + 1)
276  {
277  r = rp_min; p = rp_max;
278  } else {
279  r = rp_max; p = rp_min;
280  } */
281 
282  // Right now we use Version 1 since it reduces the overhead of interchange
283 
284  // Permutation on A
285  pivoting_2x2(k, r, p);
286  // Permutation on L
287  interchange_rows(k, p, 0, k - 1);
288  interchange_rows(k + 1, r, 0, k - 1);
289  return false;
290  }
291  }
292  }
293  }
294 
295  return true;
296  }
297 
298  // E = [e11, e12]
299  // [e21, e22]
300  // Overwrite E with inv(E)
301  void inverse_inplace_2x2(Scalar& e11, Scalar& e21, Scalar& e22) const
302  {
303  // inv(E) = [d11, d12], d11 = e22/delta, d21 = -e21/delta, d22 = e11/delta
304  // [d21, d22]
305  const Scalar delta = e11 * e22 - e21 * e21;
306  std::swap(e11, e22);
307  e11 /= delta;
308  e22 /= delta;
309  e21 = -e21 / delta;
310  }
311 
312  // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE
314  {
315  // D = 1 / A[k, k]
316  const Scalar akk = diag_coeff(k);
317  // Return NUMERICAL_ISSUE if not invertible
318  if (akk == Scalar(0))
319  return NUMERICAL_ISSUE;
320 
321  diag_coeff(k) = Scalar(1) / akk;
322 
323  // B -= l * l' / A[k, k], B := A[(k+1):end, (k+1):end], l := L[(k+1):end, k]
324  Scalar* lptr = col_pointer(k) + 1;
325  const Index ldim = m_n - k - 1;
326  MapVec l(lptr, ldim);
327  for (Index j = 0; j < ldim; j++)
328  {
329  MapVec(col_pointer(j + k + 1), ldim - j).noalias() -= (lptr[j] / akk) * l.tail(ldim - j);
330  }
331 
332  // l /= A[k, k]
333  l /= akk;
334 
335  return SUCCESSFUL;
336  }
337 
338  // Return value is the status, SUCCESSFUL/NUMERICAL_ISSUE
340  {
341  // D = inv(E)
342  Scalar& e11 = diag_coeff(k);
343  Scalar& e21 = coeff(k + 1, k);
344  Scalar& e22 = diag_coeff(k + 1);
345  // Return NUMERICAL_ISSUE if not invertible
346  if (e11 * e22 - e21 * e21 == Scalar(0))
347  return NUMERICAL_ISSUE;
348 
349  inverse_inplace_2x2(e11, e21, e22);
350 
351  // X = l * inv(E), l := L[(k+2):end, k:(k+1)]
352  Scalar* l1ptr = &coeff(k + 2, k);
353  Scalar* l2ptr = &coeff(k + 2, k + 1);
354  const Index ldim = m_n - k - 2;
355  MapVec l1(l1ptr, ldim), l2(l2ptr, ldim);
356 
358  X.col(0).noalias() = l1 * e11 + l2 * e21;
359  X.col(1).noalias() = l1 * e21 + l2 * e22;
360 
361  // B -= l * inv(E) * l' = X * l', B = A[(k+2):end, (k+2):end]
362  for (Index j = 0; j < ldim; j++)
363  {
364  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]);
365  }
366 
367  // l = X
368  l1.noalias() = X.col(0);
369  l2.noalias() = X.col(1);
370 
371  return SUCCESSFUL;
372  }
373 
374 public:
375  BKLDLT() :
376  m_n(0), m_computed(false), m_info(NOT_COMPUTED)
377  {}
378 
379  // Factorize mat - shift * I
380  BKLDLT(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0)) :
381  m_n(mat.rows()), m_computed(false), m_info(NOT_COMPUTED)
382  {
383  compute(mat, uplo, shift);
384  }
385 
386  void compute(ConstGenericMatrix& mat, int uplo = Eigen::Lower, const Scalar& shift = Scalar(0))
387  {
388  using std::abs;
389 
390  m_n = mat.rows();
391  if (m_n != mat.cols())
392  throw std::invalid_argument("BKLDLT: matrix must be square");
393 
394  m_perm.setLinSpaced(m_n, 0, m_n - 1);
395  m_permc.clear();
396 
397  // Copy data
398  m_data.resize((m_n * (m_n + 1)) / 2);
399  compute_pointer();
400  copy_data(mat, uplo, shift);
401 
402  const Scalar alpha = (1.0 + std::sqrt(17.0)) / 8.0;
403  Index k = 0;
404  for (k = 0; k < m_n - 1; k++)
405  {
406  // 1. Interchange rows and columns of A, and save the result to m_perm
407  bool is_1x1 = permutate_mat(k, alpha);
408 
409  // 2. Gaussian elimination
410  if (is_1x1)
411  {
412  m_info = gaussian_elimination_1x1(k);
413  }
414  else
415  {
416  m_info = gaussian_elimination_2x2(k);
417  k++;
418  }
419 
420  // 3. Check status
421  if (m_info != SUCCESSFUL)
422  break;
423  }
424  // Invert the last 1x1 block if it exists
425  if (k == m_n - 1)
426  {
427  const Scalar akk = diag_coeff(k);
428  if (akk == Scalar(0))
429  m_info = NUMERICAL_ISSUE;
430 
431  diag_coeff(k) = Scalar(1) / diag_coeff(k);
432  }
433 
435 
436  m_computed = true;
437  }
438 
439  // Solve Ax=b
440  void solve_inplace(GenericVector b) const
441  {
442  if (!m_computed)
443  throw std::logic_error("BKLDLT: need to call compute() first");
444 
445  // PAP' = LDL'
446  // 1. b -> Pb
447  Scalar* x = b.data();
448  MapVec res(x, m_n);
449  Index npermc = m_permc.size();
450  for (Index i = 0; i < npermc; i++)
451  {
452  std::swap(x[m_permc[i].first], x[m_permc[i].second]);
453  }
454 
455  // 2. Lz = Pb
456  // If m_perm[end] < 0, then end with m_n - 3, otherwise end with m_n - 2
457  const Index end = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2);
458  for (Index i = 0; i <= end; i++)
459  {
460  const Index b1size = m_n - i - 1;
461  const Index b2size = b1size - 1;
462  if (m_perm[i] >= 0)
463  {
464  MapConstVec l(&coeff(i + 1, i), b1size);
465  res.segment(i + 1, b1size).noalias() -= l * x[i];
466  }
467  else
468  {
469  MapConstVec l1(&coeff(i + 2, i), b2size);
470  MapConstVec l2(&coeff(i + 2, i + 1), b2size);
471  res.segment(i + 2, b2size).noalias() -= (l1 * x[i] + l2 * x[i + 1]);
472  i++;
473  }
474  }
475 
476  // 3. Dw = z
477  for (Index i = 0; i < m_n; i++)
478  {
479  const Scalar e11 = diag_coeff(i);
480  if (m_perm[i] >= 0)
481  {
482  x[i] *= e11;
483  }
484  else
485  {
486  const Scalar e21 = coeff(i + 1, i), e22 = diag_coeff(i + 1);
487  const Scalar wi = x[i] * e11 + x[i + 1] * e21;
488  x[i + 1] = x[i] * e21 + x[i + 1] * e22;
489  x[i] = wi;
490  i++;
491  }
492  }
493 
494  // 4. L'y = w
495  // If m_perm[end] < 0, then start with m_n - 3, otherwise start with m_n - 2
496  Index i = (m_perm[m_n - 1] < 0) ? (m_n - 3) : (m_n - 2);
497  for (; i >= 0; i--)
498  {
499  const Index ldim = m_n - i - 1;
500  MapConstVec l(&coeff(i + 1, i), ldim);
501  x[i] -= res.segment(i + 1, ldim).dot(l);
502 
503  if (m_perm[i] < 0)
504  {
505  MapConstVec l2(&coeff(i + 1, i - 1), ldim);
506  x[i - 1] -= res.segment(i + 1, ldim).dot(l2);
507  i--;
508  }
509  }
510 
511  // 5. x = P'y
512  for (Index i = npermc - 1; i >= 0; i--)
513  {
514  std::swap(x[m_permc[i].first], x[m_permc[i].second]);
515  }
516  }
517 
518  Vector solve(ConstGenericVector& b) const
519  {
520  Vector res = b;
521  solve_inplace(res);
522  return res;
523  }
524 
525  int info() const { return m_info; }
526 };
527 
528 } // namespace Spectra
529 
530 #endif // BK_LDLT_H
Eigen::Ref< Vector > GenericVector
Definition: BKLDLT.h:37
Vector m_data
Definition: BKLDLT.h:43
SCALAR Scalar
Definition: bench_gemm.cpp:46
void compute(ConstGenericMatrix &mat, int uplo=Eigen::Lower, const Scalar &shift=Scalar(0))
Definition: BKLDLT.h:386
void solve_inplace(GenericVector b) const
Definition: BKLDLT.h:440
std::vector< std::pair< Index, Index > > m_permc
Definition: BKLDLT.h:46
Scalar * b
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
const Eigen::Ref< const Vector > ConstGenericVector
Definition: BKLDLT.h:40
void inverse_inplace_2x2(Scalar &e11, Scalar &e21, Scalar &e22) const
Definition: BKLDLT.h:301
Scalar find_sigma(Index k, Index r, Index &p)
Definition: BKLDLT.h:203
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
gtsam::Key l2
Eigen::Ref< Matrix > GenericMatrix
Definition: BKLDLT.h:38
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: BKLDLT.h:31
bool permutate_mat(Index k, const Scalar &alpha)
Definition: BKLDLT.h:229
const Scalar & diag_coeff(Index i) const
Definition: BKLDLT.h:59
Computation was successful.
Definition: CompInfo.h:19
const Scalar & coeff(Index i, Index j) const
Definition: BKLDLT.h:56
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: BKLDLT.h:32
Scalar * col_pointer(Index k)
Definition: BKLDLT.h:53
int gaussian_elimination_1x1(Index k)
Definition: BKLDLT.h:313
const Eigen::Ref< const Matrix > ConstGenericMatrix
Definition: BKLDLT.h:39
cout<< "Here is the matrix m:"<< endl<< m<< endl;Matrix< ptrdiff_t, 3, 1 > res
Scalar & coeff(Index i, Index j)
Definition: BKLDLT.h:55
Eigen::Matrix< Index, Eigen::Dynamic, 1 > IntVector
Definition: BKLDLT.h:36
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
Index m_n
Definition: BKLDLT.h:42
Vector solve(ConstGenericVector &b) const
Definition: BKLDLT.h:518
void compress_permutation()
Definition: BKLDLT.h:103
int info() const
Definition: BKLDLT.h:525
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
void interchange_rows(Index r1, Index r2, Index c1, Index c2)
Definition: BKLDLT.h:161
idx_t idx_t idx_t idx_t idx_t * perm
RealScalar alpha
static const double r2
Scalar & diag_coeff(Index i)
Definition: BKLDLT.h:58
Scalar find_lambda(Index k, Index &r)
Definition: BKLDLT.h:176
void copy_data(ConstGenericMatrix &mat, int uplo, const Scalar &shift)
Definition: BKLDLT.h:76
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
void pivoting_2x2(Index k, Index r, Index p)
Definition: BKLDLT.h:145
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
bool m_computed
Definition: BKLDLT.h:48
gtsam::Key l1
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
void pivoting_1x1(Index k, Index r)
Definition: BKLDLT.h:117
static const double r1
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
Eigen::Map< const Vector > MapConstVec
Definition: BKLDLT.h:34
static EIGEN_DEPRECATED const end_t end
float * p
void compute_pointer()
Definition: BKLDLT.h:62
std::vector< Scalar * > m_colptr
Definition: BKLDLT.h:44
static const double sigma
Eigen::Map< Vector > MapVec
Definition: BKLDLT.h:33
IntVector m_perm
Definition: BKLDLT.h:45
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
#define X
Definition: icosphere.cpp:20
size_t len(handle h)
Get the length of a Python object.
Definition: pytypes.h:2244
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
#define abs(x)
Definition: datatypes.h:17
int gaussian_elimination_2x2(Index k)
Definition: BKLDLT.h:339
Eigen::Index Index
Definition: BKLDLT.h:30
std::ptrdiff_t j
BKLDLT(ConstGenericMatrix &mat, int uplo=Eigen::Lower, const Scalar &shift=Scalar(0))
Definition: BKLDLT.h:380


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:58