UpperHessenbergQR.h
Go to the documentation of this file.
1 // Copyright (C) 2016-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 UPPER_HESSENBERG_QR_H
8 #define UPPER_HESSENBERG_QR_H
9 
10 #include <Eigen/Core>
11 #include <cmath> // std::sqrt
12 #include <algorithm> // std::fill, std::copy
13 #include <stdexcept> // std::logic_error
14 
15 namespace Spectra {
16 
22 
27 
33 
42 template <typename Scalar = double>
44 {
45 private:
51 
54 
55  Matrix m_mat_T;
56 
57 protected:
58  Index m_n;
59  // Gi = [ cos[i] sin[i]]
60  // [-sin[i] cos[i]]
61  // Q = G1 * G2 * ... * G_{n-1}
63  Array m_rot_cos;
64  Array m_rot_sin;
65  bool m_computed;
66 
67  // Given x and y, compute 1) r = sqrt(x^2 + y^2), 2) c = x / r, 3) s = -y / r
68  // If both x and y are zero, set c = 1 and s = 0
69  // We must implement it in a numerically stable way
70  static void compute_rotation(const Scalar& x, const Scalar& y, Scalar& r, Scalar& c, Scalar& s)
71  {
72  using std::sqrt;
73 
74  const Scalar xsign = (x > Scalar(0)) - (x < Scalar(0));
75  const Scalar ysign = (y > Scalar(0)) - (y < Scalar(0));
76  const Scalar xabs = x * xsign;
77  const Scalar yabs = y * ysign;
78  if (xabs > yabs)
79  {
80  // In this case xabs != 0
81  const Scalar ratio = yabs / xabs; // so that 0 <= ratio < 1
82  const Scalar common = sqrt(Scalar(1) + ratio * ratio);
83  c = xsign / common;
84  r = xabs * common;
85  s = -y / r;
86  }
87  else
88  {
89  if (yabs == Scalar(0))
90  {
91  r = Scalar(0);
92  c = Scalar(1);
93  s = Scalar(0);
94  return;
95  }
96  const Scalar ratio = xabs / yabs; // so that 0 <= ratio <= 1
97  const Scalar common = sqrt(Scalar(1) + ratio * ratio);
98  s = -ysign / common;
99  r = yabs * common;
100  c = x / r;
101  }
102  }
103 
104 public:
110  m_n(size),
111  m_rot_cos(m_n - 1),
112  m_rot_sin(m_n - 1),
113  m_computed(false)
114  {}
115 
128  UpperHessenbergQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) :
129  m_n(mat.rows()),
130  m_shift(shift),
131  m_rot_cos(m_n - 1),
132  m_rot_sin(m_n - 1),
133  m_computed(false)
134  {
135  compute(mat, shift);
136  }
137 
141  virtual ~UpperHessenbergQR(){};
142 
153  virtual void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0))
154  {
155  m_n = mat.rows();
156  if (m_n != mat.cols())
157  throw std::invalid_argument("UpperHessenbergQR: matrix must be square");
158 
159  m_shift = shift;
160  m_mat_T.resize(m_n, m_n);
161  m_rot_cos.resize(m_n - 1);
162  m_rot_sin.resize(m_n - 1);
163 
164  // Make a copy of mat - s * I
165  std::copy(mat.data(), mat.data() + mat.size(), m_mat_T.data());
166  m_mat_T.diagonal().array() -= m_shift;
167 
168  Scalar xi, xj, r, c, s;
169  Scalar *Tii, *ptr;
170  const Index n1 = m_n - 1;
171  for (Index i = 0; i < n1; i++)
172  {
173  Tii = &m_mat_T.coeffRef(i, i);
174 
175  // Make sure mat_T is upper Hessenberg
176  // Zero the elements below mat_T(i + 1, i)
177  std::fill(Tii + 2, Tii + m_n - i, Scalar(0));
178 
179  xi = Tii[0]; // mat_T(i, i)
180  xj = Tii[1]; // mat_T(i + 1, i)
181  compute_rotation(xi, xj, r, c, s);
182  m_rot_cos[i] = c;
183  m_rot_sin[i] = s;
184 
185  // For a complete QR decomposition,
186  // we first obtain the rotation matrix
187  // G = [ cos sin]
188  // [-sin cos]
189  // and then do T[i:(i + 1), i:(n - 1)] = G' * T[i:(i + 1), i:(n - 1)]
190 
191  // Gt << c, -s, s, c;
192  // m_mat_T.block(i, i, 2, m_n - i) = Gt * m_mat_T.block(i, i, 2, m_n - i);
193  Tii[0] = r; // m_mat_T(i, i) => r
194  Tii[1] = 0; // m_mat_T(i + 1, i) => 0
195  ptr = Tii + m_n; // m_mat_T(i, k), k = i+1, i+2, ..., n-1
196  for (Index j = i + 1; j < m_n; j++, ptr += m_n)
197  {
198  Scalar tmp = ptr[0];
199  ptr[0] = c * tmp - s * ptr[1];
200  ptr[1] = s * tmp + c * ptr[1];
201  }
202 
203  // If we do not need to calculate the R matrix, then
204  // only the cos and sin sequences are required.
205  // In such case we only update T[i + 1, (i + 1):(n - 1)]
206  // m_mat_T.block(i + 1, i + 1, 1, m_n - i - 1) *= c;
207  // 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);
208  }
209 
210  m_computed = true;
211  }
212 
220  virtual Matrix matrix_R() const
221  {
222  if (!m_computed)
223  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
224 
225  return m_mat_T;
226  }
227 
235  virtual void matrix_QtHQ(Matrix& dest) const
236  {
237  if (!m_computed)
238  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
239 
240  // Make a copy of the R matrix
241  dest.resize(m_n, m_n);
242  std::copy(m_mat_T.data(), m_mat_T.data() + m_mat_T.size(), dest.data());
243 
244  // Compute the RQ matrix
245  const Index n1 = m_n - 1;
246  for (Index i = 0; i < n1; i++)
247  {
248  const Scalar c = m_rot_cos.coeff(i);
249  const Scalar s = m_rot_sin.coeff(i);
250  // RQ[, i:(i + 1)] = RQ[, i:(i + 1)] * Gi
251  // Gi = [ cos[i] sin[i]]
252  // [-sin[i] cos[i]]
253  Scalar *Yi, *Yi1;
254  Yi = &dest.coeffRef(0, i);
255  Yi1 = Yi + m_n; // RQ(0, i + 1)
256  const Index i2 = i + 2;
257  for (Index j = 0; j < i2; j++)
258  {
259  const Scalar tmp = Yi[j];
260  Yi[j] = c * tmp - s * Yi1[j];
261  Yi1[j] = s * tmp + c * Yi1[j];
262  }
263 
264  /* Vector dest = RQ.block(0, i, i + 2, 1);
265  dest.block(0, i, i + 2, 1) = c * Yi - s * dest.block(0, i + 1, i + 2, 1);
266  dest.block(0, i + 1, i + 2, 1) = s * Yi + c * dest.block(0, i + 1, i + 2, 1); */
267  }
268 
269  // Add the shift to the diagonal
270  dest.diagonal().array() += m_shift;
271  }
272 
281  // Y -> QY = G1 * G2 * ... * Y
282  void apply_QY(Vector& Y) const
283  {
284  if (!m_computed)
285  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
286 
287  for (Index i = m_n - 2; i >= 0; i--)
288  {
289  const Scalar c = m_rot_cos.coeff(i);
290  const Scalar s = m_rot_sin.coeff(i);
291  // Y[i:(i + 1)] = Gi * Y[i:(i + 1)]
292  // Gi = [ cos[i] sin[i]]
293  // [-sin[i] cos[i]]
294  const Scalar tmp = Y[i];
295  Y[i] = c * tmp + s * Y[i + 1];
296  Y[i + 1] = -s * tmp + c * Y[i + 1];
297  }
298  }
299 
308  // Y -> Q'Y = G_{n-1}' * ... * G2' * G1' * Y
309  void apply_QtY(Vector& Y) const
310  {
311  if (!m_computed)
312  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
313 
314  const Index n1 = m_n - 1;
315  for (Index i = 0; i < n1; i++)
316  {
317  const Scalar c = m_rot_cos.coeff(i);
318  const Scalar s = m_rot_sin.coeff(i);
319  // Y[i:(i + 1)] = Gi' * Y[i:(i + 1)]
320  // Gi = [ cos[i] sin[i]]
321  // [-sin[i] cos[i]]
322  const Scalar tmp = Y[i];
323  Y[i] = c * tmp - s * Y[i + 1];
324  Y[i + 1] = s * tmp + c * Y[i + 1];
325  }
326  }
327 
337  // Y -> QY = G1 * G2 * ... * Y
338  void apply_QY(GenericMatrix Y) const
339  {
340  if (!m_computed)
341  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
342 
343  RowVector Yi(Y.cols()), Yi1(Y.cols());
344  for (Index i = m_n - 2; i >= 0; i--)
345  {
346  const Scalar c = m_rot_cos.coeff(i);
347  const Scalar s = m_rot_sin.coeff(i);
348  // Y[i:(i + 1), ] = Gi * Y[i:(i + 1), ]
349  // Gi = [ cos[i] sin[i]]
350  // [-sin[i] cos[i]]
351  Yi.noalias() = Y.row(i);
352  Yi1.noalias() = Y.row(i + 1);
353  Y.row(i) = c * Yi + s * Yi1;
354  Y.row(i + 1) = -s * Yi + c * Yi1;
355  }
356  }
357 
367  // Y -> Q'Y = G_{n-1}' * ... * G2' * G1' * Y
368  void apply_QtY(GenericMatrix Y) const
369  {
370  if (!m_computed)
371  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
372 
373  RowVector Yi(Y.cols()), Yi1(Y.cols());
374  const Index n1 = m_n - 1;
375  for (Index i = 0; i < n1; i++)
376  {
377  const Scalar c = m_rot_cos.coeff(i);
378  const Scalar s = m_rot_sin.coeff(i);
379  // Y[i:(i + 1), ] = Gi' * Y[i:(i + 1), ]
380  // Gi = [ cos[i] sin[i]]
381  // [-sin[i] cos[i]]
382  Yi.noalias() = Y.row(i);
383  Yi1.noalias() = Y.row(i + 1);
384  Y.row(i) = c * Yi - s * Yi1;
385  Y.row(i + 1) = s * Yi + c * Yi1;
386  }
387  }
388 
398  // Y -> YQ = Y * G1 * G2 * ...
399  void apply_YQ(GenericMatrix Y) const
400  {
401  if (!m_computed)
402  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
403 
404  /*Vector Yi(Y.rows());
405  for(Index i = 0; i < m_n - 1; i++)
406  {
407  const Scalar c = m_rot_cos.coeff(i);
408  const Scalar s = m_rot_sin.coeff(i);
409  // Y[, i:(i + 1)] = Y[, i:(i + 1)] * Gi
410  // Gi = [ cos[i] sin[i]]
411  // [-sin[i] cos[i]]
412  Yi.noalias() = Y.col(i);
413  Y.col(i) = c * Yi - s * Y.col(i + 1);
414  Y.col(i + 1) = s * Yi + c * Y.col(i + 1);
415  }*/
416  Scalar *Y_col_i, *Y_col_i1;
417  const Index n1 = m_n - 1;
418  const Index nrow = Y.rows();
419  for (Index i = 0; i < n1; i++)
420  {
421  const Scalar c = m_rot_cos.coeff(i);
422  const Scalar s = m_rot_sin.coeff(i);
423 
424  Y_col_i = &Y.coeffRef(0, i);
425  Y_col_i1 = &Y.coeffRef(0, i + 1);
426  for (Index j = 0; j < nrow; j++)
427  {
428  Scalar tmp = Y_col_i[j];
429  Y_col_i[j] = c * tmp - s * Y_col_i1[j];
430  Y_col_i1[j] = s * tmp + c * Y_col_i1[j];
431  }
432  }
433  }
434 
444  // Y -> YQ' = Y * G_{n-1}' * ... * G2' * G1'
445  void apply_YQt(GenericMatrix Y) const
446  {
447  if (!m_computed)
448  throw std::logic_error("UpperHessenbergQR: need to call compute() first");
449 
450  Vector Yi(Y.rows());
451  for (Index i = m_n - 2; i >= 0; i--)
452  {
453  const Scalar c = m_rot_cos.coeff(i);
454  const Scalar s = m_rot_sin.coeff(i);
455  // Y[, i:(i + 1)] = Y[, i:(i + 1)] * Gi'
456  // Gi = [ cos[i] sin[i]]
457  // [-sin[i] cos[i]]
458  Yi.noalias() = Y.col(i);
459  Y.col(i) = c * Yi + s * Y.col(i + 1);
460  Y.col(i + 1) = -s * Yi + c * Y.col(i + 1);
461  }
462  }
463 };
464 
474 template <typename Scalar = double>
475 class TridiagQR : public UpperHessenbergQR<Scalar>
476 {
477 private:
481 
482  typedef typename Matrix::Index Index;
483 
484  Vector m_T_diag; // diagonal elements of T
485  Vector m_T_lsub; // lower subdiagonal of T
486  Vector m_T_usub; // upper subdiagonal of T
487  Vector m_T_usub2; // 2nd upper subdiagonal of T
488 
489 public:
494  TridiagQR(Index size) :
496  {}
497 
510  TridiagQR(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0)) :
512  {
513  this->compute(mat, shift);
514  }
515 
526  void compute(ConstGenericMatrix& mat, const Scalar& shift = Scalar(0))
527  {
528  this->m_n = mat.rows();
529  if (this->m_n != mat.cols())
530  throw std::invalid_argument("TridiagQR: matrix must be square");
531 
532  this->m_shift = shift;
533  m_T_diag.resize(this->m_n);
534  m_T_lsub.resize(this->m_n - 1);
535  m_T_usub.resize(this->m_n - 1);
536  m_T_usub2.resize(this->m_n - 2);
537  this->m_rot_cos.resize(this->m_n - 1);
538  this->m_rot_sin.resize(this->m_n - 1);
539 
540  m_T_diag.array() = mat.diagonal().array() - this->m_shift;
541  m_T_lsub.noalias() = mat.diagonal(-1);
542  m_T_usub.noalias() = m_T_lsub;
543 
544  // A number of pointers to avoid repeated address calculation
545  Scalar *c = this->m_rot_cos.data(), // pointer to the cosine vector
546  *s = this->m_rot_sin.data(), // pointer to the sine vector
547  r;
548  const Index n1 = this->m_n - 1;
549  for (Index i = 0; i < n1; i++)
550  {
551  // diag[i] == T[i, i]
552  // lsub[i] == T[i + 1, i]
553  // r = sqrt(T[i, i]^2 + T[i + 1, i]^2)
554  // c = T[i, i] / r, s = -T[i + 1, i] / r
555  this->compute_rotation(m_T_diag.coeff(i), m_T_lsub.coeff(i), r, *c, *s);
556 
557  // For a complete QR decomposition,
558  // we first obtain the rotation matrix
559  // G = [ cos sin]
560  // [-sin cos]
561  // and then do T[i:(i + 1), i:(i + 2)] = G' * T[i:(i + 1), i:(i + 2)]
562 
563  // Update T[i, i] and T[i + 1, i]
564  // The updated value of T[i, i] is known to be r
565  // The updated value of T[i + 1, i] is known to be 0
566  m_T_diag.coeffRef(i) = r;
567  m_T_lsub.coeffRef(i) = Scalar(0);
568  // Update T[i, i + 1] and T[i + 1, i + 1]
569  // usub[i] == T[i, i + 1]
570  // diag[i + 1] == T[i + 1, i + 1]
571  const Scalar tmp = m_T_usub.coeff(i);
572  m_T_usub.coeffRef(i) = (*c) * tmp - (*s) * m_T_diag.coeff(i + 1);
573  m_T_diag.coeffRef(i + 1) = (*s) * tmp + (*c) * m_T_diag.coeff(i + 1);
574  // Update T[i, i + 2] and T[i + 1, i + 2]
575  // usub2[i] == T[i, i + 2]
576  // usub[i + 1] == T[i + 1, i + 2]
577  if (i < n1 - 1)
578  {
579  m_T_usub2.coeffRef(i) = -(*s) * m_T_usub.coeff(i + 1);
580  m_T_usub.coeffRef(i + 1) *= (*c);
581  }
582 
583  c++;
584  s++;
585 
586  // If we do not need to calculate the R matrix, then
587  // only the cos and sin sequences are required.
588  // In such case we only update T[i + 1, (i + 1):(i + 2)]
589  // T[i + 1, i + 1] = c * T[i + 1, i + 1] + s * T[i, i + 1];
590  // T[i + 1, i + 2] *= c;
591  }
592 
593  this->m_computed = true;
594  }
595 
603  Matrix matrix_R() const
604  {
605  if (!this->m_computed)
606  throw std::logic_error("TridiagQR: need to call compute() first");
607 
608  Matrix R = Matrix::Zero(this->m_n, this->m_n);
609  R.diagonal().noalias() = m_T_diag;
610  R.diagonal(1).noalias() = m_T_usub;
611  R.diagonal(2).noalias() = m_T_usub2;
612 
613  return R;
614  }
615 
623  void matrix_QtHQ(Matrix& dest) const
624  {
625  if (!this->m_computed)
626  throw std::logic_error("TridiagQR: need to call compute() first");
627 
628  // Make a copy of the R matrix
629  dest.resize(this->m_n, this->m_n);
630  dest.setZero();
631  dest.diagonal().noalias() = m_T_diag;
632  // The upper diagonal refers to m_T_usub
633  // The 2nd upper subdiagonal will be zero in RQ
634 
635  // Compute the RQ matrix
636  // [m11 m12] points to RQ[i:(i+1), i:(i+1)]
637  // [0 m22]
638  //
639  // Gi = [ cos[i] sin[i]]
640  // [-sin[i] cos[i]]
641  const Index n1 = this->m_n - 1;
642  for (Index i = 0; i < n1; i++)
643  {
644  const Scalar c = this->m_rot_cos.coeff(i);
645  const Scalar s = this->m_rot_sin.coeff(i);
646  const Scalar m11 = dest.coeff(i, i),
647  m12 = m_T_usub.coeff(i),
648  m22 = m_T_diag.coeff(i + 1);
649 
650  // Update the diagonal and the lower subdiagonal of dest
651  dest.coeffRef(i, i) = c * m11 - s * m12;
652  dest.coeffRef(i + 1, i) = -s * m22;
653  dest.coeffRef(i + 1, i + 1) = c * m22;
654  }
655 
656  // Copy the lower subdiagonal to upper subdiagonal
657  dest.diagonal(1).noalias() = dest.diagonal(-1);
658 
659  // Add the shift to the diagonal
660  dest.diagonal().array() += this->m_shift;
661  }
662 };
663 
667 
668 } // namespace Spectra
669 
670 #endif // UPPER_HESSENBERG_QR_H
virtual void compute(ConstGenericMatrix &mat, const Scalar &shift=Scalar(0))
SCALAR Scalar
Definition: bench_gemm.cpp:46
const char Y
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
Scalar * y
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
void compute(ConstGenericMatrix &mat, const Scalar &shift=Scalar(0))
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
void apply_YQt(GenericMatrix Y) const
Rot2 R(Rot2::fromAngle(0.1))
void apply_QtY(Vector &Y) const
const Eigen::Ref< const Matrix > ConstGenericMatrix
UpperHessenbergQR(ConstGenericMatrix &mat, const Scalar &shift=Scalar(0))
Eigen::Array< Scalar, Eigen::Dynamic, 1 > Array
virtual Matrix matrix_R() const
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
Eigen::Ref< Matrix > GenericMatrix
virtual void matrix_QtHQ(Matrix &dest) const
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
void apply_QY(GenericMatrix Y) const
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar & coeff(Index rowId, Index colId) const
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
void apply_QY(Vector &Y) const
RealScalar s
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Matrix matrix_R() const
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Vector xi
Definition: testPose2.cpp:148
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
const Eigen::Ref< const Matrix > ConstGenericMatrix
void matrix_QtHQ(Matrix &dest) const
Eigen::Matrix< Scalar, 1, Eigen::Dynamic > RowVector
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
void apply_YQ(GenericMatrix Y) const
TridiagQR(ConstGenericMatrix &mat, const Scalar &shift=Scalar(0))
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
void apply_QtY(GenericMatrix Y) const
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
std::ptrdiff_t j
static void compute_rotation(const Scalar &x, const Scalar &y, Scalar &r, Scalar &c, Scalar &s)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:43