DoubleShiftQR.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 DOUBLE_SHIFT_QR_H
8 #define DOUBLE_SHIFT_QR_H
9 
10 #include <Eigen/Core>
11 #include <vector> // std::vector
12 #include <algorithm> // std::min, std::fill, std::copy
13 #include <cmath> // std::abs, std::sqrt, std::pow
14 #include <stdexcept> // std::invalid_argument, std::logic_error
15 
16 #include "../Util/TypeTraits.h"
17 
18 namespace Spectra {
19 
20 template <typename Scalar = double>
22 {
23 private:
29 
32 
33  Index m_n; // Dimension of the matrix
34  Matrix m_mat_H; // A copy of the matrix to be factorized
35  Scalar m_shift_s; // Shift constant
36  Scalar m_shift_t; // Shift constant
37  Matrix3X m_ref_u; // Householder reflectors
38  IntArray m_ref_nr; // How many rows does each reflector affects
39  // 3 - A general reflector
40  // 2 - A Givens rotation
41  // 1 - An identity transformation
42  const Scalar m_near_0; // a very small value, but 1.0 / m_safe_min does not overflow
43  // ~= 1e-307 for the "double" type
44  const Scalar m_eps; // the machine precision,
45  // e.g. ~= 1e-16 for the "double" type
48  bool m_computed; // Whether matrix has been factorized
49 
50  void compute_reflector(const Scalar& x1, const Scalar& x2, const Scalar& x3, Index ind)
51  {
52  using std::abs;
53 
54  Scalar* u = &m_ref_u.coeffRef(0, ind);
55  unsigned char* nr = m_ref_nr.data();
56  // In general case the reflector affects 3 rows
57  nr[ind] = 3;
58  Scalar x2x3 = Scalar(0);
59  // If x3 is zero, decrease nr by 1
60  if (abs(x3) < m_near_0)
61  {
62  // If x2 is also zero, nr will be 1, and we can exit this function
63  if (abs(x2) < m_near_0)
64  {
65  nr[ind] = 1;
66  return;
67  }
68  else
69  {
70  nr[ind] = 2;
71  }
72  x2x3 = abs(x2);
73  }
74  else
75  {
76  x2x3 = Eigen::numext::hypot(x2, x3);
77  }
78 
79  // x1' = x1 - rho * ||x||
80  // rho = -sign(x1), if x1 == 0, we choose rho = 1
81  Scalar x1_new = x1 - ((x1 <= 0) - (x1 > 0)) * Eigen::numext::hypot(x1, x2x3);
82  Scalar x_norm = Eigen::numext::hypot(x1_new, x2x3);
83  // Double check the norm of new x
84  if (x_norm < m_near_0)
85  {
86  nr[ind] = 1;
87  return;
88  }
89  u[0] = x1_new / x_norm;
90  u[1] = x2 / x_norm;
91  u[2] = x3 / x_norm;
92  }
93 
94  void compute_reflector(const Scalar* x, Index ind)
95  {
96  compute_reflector(x[0], x[1], x[2], ind);
97  }
98 
99  // Update the block X = H(il:iu, il:iu)
100  void update_block(Index il, Index iu)
101  {
102  // Block size
103  const Index bsize = iu - il + 1;
104 
105  // If block size == 1, there is no need to apply reflectors
106  if (bsize == 1)
107  {
108  m_ref_nr.coeffRef(il) = 1;
109  return;
110  }
111 
112  const Scalar x00 = m_mat_H.coeff(il, il),
113  x01 = m_mat_H.coeff(il, il + 1),
114  x10 = m_mat_H.coeff(il + 1, il),
115  x11 = m_mat_H.coeff(il + 1, il + 1);
116  // m00 = x00 * (x00 - s) + x01 * x10 + t
117  const Scalar m00 = x00 * (x00 - m_shift_s) + x01 * x10 + m_shift_t;
118  // m10 = x10 * (x00 + x11 - s)
119  const Scalar m10 = x10 * (x00 + x11 - m_shift_s);
120 
121  // For block size == 2, do a Givens rotation on M = X * X - s * X + t * I
122  if (bsize == 2)
123  {
124  // This causes nr=2
125  compute_reflector(m00, m10, 0, il);
126  // Apply the reflector to X
127  apply_PX(m_mat_H.block(il, il, 2, m_n - il), m_n, il);
128  apply_XP(m_mat_H.block(0, il, il + 2, 2), m_n, il);
129 
130  m_ref_nr.coeffRef(il + 1) = 1;
131  return;
132  }
133 
134  // For block size >=3, use the regular strategy
135  // m20 = x21 * x10
136  const Scalar m20 = m_mat_H.coeff(il + 2, il + 1) * m_mat_H.coeff(il + 1, il);
137  compute_reflector(m00, m10, m20, il);
138 
139  // Apply the first reflector
140  apply_PX(m_mat_H.block(il, il, 3, m_n - il), m_n, il);
141  apply_XP(m_mat_H.block(0, il, il + std::min(bsize, Index(4)), 3), m_n, il);
142 
143  // Calculate the following reflectors
144  // If entering this loop, block size is at least 4.
145  for (Index i = 1; i < bsize - 2; i++)
146  {
147  compute_reflector(&m_mat_H.coeffRef(il + i, il + i - 1), il + i);
148  // Apply the reflector to X
149  apply_PX(m_mat_H.block(il + i, il + i - 1, 3, m_n - il - i + 1), m_n, il + i);
150  apply_XP(m_mat_H.block(0, il + i, il + std::min(bsize, Index(i + 4)), 3), m_n, il + i);
151  }
152 
153  // The last reflector
154  // This causes nr=2
155  compute_reflector(m_mat_H.coeff(iu - 1, iu - 2), m_mat_H.coeff(iu, iu - 2), 0, iu - 1);
156  // Apply the reflector to X
157  apply_PX(m_mat_H.block(iu - 1, iu - 2, 2, m_n - iu + 2), m_n, iu - 1);
158  apply_XP(m_mat_H.block(0, iu - 1, il + bsize, 2), m_n, iu - 1);
159 
160  m_ref_nr.coeffRef(iu) = 1;
161  }
162 
163  // P = I - 2 * u * u' = P'
164  // PX = X - 2 * u * (u'X)
165  void apply_PX(GenericMatrix X, Index stride, Index u_ind) const
166  {
167  const Index nr = m_ref_nr.coeff(u_ind);
168  if (nr == 1)
169  return;
170 
171  const Scalar u0 = m_ref_u.coeff(0, u_ind),
172  u1 = m_ref_u.coeff(1, u_ind);
173  const Scalar u0_2 = Scalar(2) * u0,
174  u1_2 = Scalar(2) * u1;
175 
176  const Index nrow = X.rows();
177  const Index ncol = X.cols();
178 
179  Scalar* xptr = X.data();
180  if (nr == 2 || nrow == 2)
181  {
182  for (Index i = 0; i < ncol; i++, xptr += stride)
183  {
184  const Scalar tmp = u0_2 * xptr[0] + u1_2 * xptr[1];
185  xptr[0] -= tmp * u0;
186  xptr[1] -= tmp * u1;
187  }
188  }
189  else
190  {
191  const Scalar u2 = m_ref_u.coeff(2, u_ind);
192  const Scalar u2_2 = Scalar(2) * u2;
193  for (Index i = 0; i < ncol; i++, xptr += stride)
194  {
195  const Scalar tmp = u0_2 * xptr[0] + u1_2 * xptr[1] + u2_2 * xptr[2];
196  xptr[0] -= tmp * u0;
197  xptr[1] -= tmp * u1;
198  xptr[2] -= tmp * u2;
199  }
200  }
201  }
202 
203  // x is a pointer to a vector
204  // Px = x - 2 * dot(x, u) * u
205  void apply_PX(Scalar* x, Index u_ind) const
206  {
207  const Index nr = m_ref_nr.coeff(u_ind);
208  if (nr == 1)
209  return;
210 
211  const Scalar u0 = m_ref_u.coeff(0, u_ind),
212  u1 = m_ref_u.coeff(1, u_ind),
213  u2 = m_ref_u.coeff(2, u_ind);
214 
215  // When the reflector only contains two elements, u2 has been set to zero
216  const bool nr_is_2 = (nr == 2);
217  const Scalar dot2 = Scalar(2) * (x[0] * u0 + x[1] * u1 + (nr_is_2 ? 0 : (x[2] * u2)));
218  x[0] -= dot2 * u0;
219  x[1] -= dot2 * u1;
220  if (!nr_is_2)
221  x[2] -= dot2 * u2;
222  }
223 
224  // XP = X - 2 * (X * u) * u'
225  void apply_XP(GenericMatrix X, Index stride, Index u_ind) const
226  {
227  const Index nr = m_ref_nr.coeff(u_ind);
228  if (nr == 1)
229  return;
230 
231  const Scalar u0 = m_ref_u.coeff(0, u_ind),
232  u1 = m_ref_u.coeff(1, u_ind);
233  const Scalar u0_2 = Scalar(2) * u0,
234  u1_2 = Scalar(2) * u1;
235 
236  const int nrow = X.rows();
237  const int ncol = X.cols();
238  Scalar *X0 = X.data(), *X1 = X0 + stride; // X0 => X.col(0), X1 => X.col(1)
239 
240  if (nr == 2 || ncol == 2)
241  {
242  // tmp = 2 * u0 * X0 + 2 * u1 * X1
243  // X0 => X0 - u0 * tmp
244  // X1 => X1 - u1 * tmp
245  for (Index i = 0; i < nrow; i++)
246  {
247  const Scalar tmp = u0_2 * X0[i] + u1_2 * X1[i];
248  X0[i] -= tmp * u0;
249  X1[i] -= tmp * u1;
250  }
251  }
252  else
253  {
254  Scalar* X2 = X1 + stride; // X2 => X.col(2)
255  const Scalar u2 = m_ref_u.coeff(2, u_ind);
256  const Scalar u2_2 = Scalar(2) * u2;
257  for (Index i = 0; i < nrow; i++)
258  {
259  const Scalar tmp = u0_2 * X0[i] + u1_2 * X1[i] + u2_2 * X2[i];
260  X0[i] -= tmp * u0;
261  X1[i] -= tmp * u1;
262  X2[i] -= tmp * u2;
263  }
264  }
265  }
266 
267 public:
269  m_n(size),
270  m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
271  m_eps(Eigen::NumTraits<Scalar>::epsilon()),
272  m_eps_rel(m_eps),
273  m_eps_abs(m_near_0 * (m_n / m_eps)),
274  m_computed(false)
275  {}
276 
277  DoubleShiftQR(ConstGenericMatrix& mat, const Scalar& s, const Scalar& t) :
278  m_n(mat.rows()),
279  m_mat_H(m_n, m_n),
280  m_shift_s(s),
281  m_shift_t(t),
282  m_ref_u(3, m_n),
283  m_ref_nr(m_n),
284  m_near_0(TypeTraits<Scalar>::min() * Scalar(10)),
285  m_eps(Eigen::NumTraits<Scalar>::epsilon()),
286  m_eps_rel(m_eps),
287  m_eps_abs(m_near_0 * (m_n / m_eps)),
288  m_computed(false)
289  {
290  compute(mat, s, t);
291  }
292 
293  void compute(ConstGenericMatrix& mat, const Scalar& s, const Scalar& t)
294  {
295  using std::abs;
296 
297  m_n = mat.rows();
298  if (m_n != mat.cols())
299  throw std::invalid_argument("DoubleShiftQR: matrix must be square");
300 
301  m_mat_H.resize(m_n, m_n);
302  m_shift_s = s;
303  m_shift_t = t;
304  m_ref_u.resize(3, m_n);
305  m_ref_nr.resize(m_n);
306 
307  // Make a copy of mat
308  std::copy(mat.data(), mat.data() + mat.size(), m_mat_H.data());
309 
310  // Obtain the indices of zero elements in the subdiagonal,
311  // so that H can be divided into several blocks
312  std::vector<int> zero_ind;
313  zero_ind.reserve(m_n - 1);
314  zero_ind.push_back(0);
315  Scalar* Hii = m_mat_H.data();
316  for (Index i = 0; i < m_n - 2; i++, Hii += (m_n + 1))
317  {
318  // Hii[1] => m_mat_H(i + 1, i)
319  const Scalar h = abs(Hii[1]);
320  if (h <= 0 || h <= m_eps_rel * (abs(Hii[0]) + abs(Hii[m_n + 1])))
321  {
322  Hii[1] = 0;
323  zero_ind.push_back(i + 1);
324  }
325  // Make sure m_mat_H is upper Hessenberg
326  // Zero the elements below m_mat_H(i + 1, i)
327  std::fill(Hii + 2, Hii + m_n - i, Scalar(0));
328  }
329  zero_ind.push_back(m_n);
330 
331  for (std::vector<int>::size_type i = 0; i < zero_ind.size() - 1; i++)
332  {
333  const Index start = zero_ind[i];
334  const Index end = zero_ind[i + 1] - 1;
335  // Compute refelctors and update each block
336  update_block(start, end);
337  }
338 
339  m_computed = true;
340  }
341 
342  void matrix_QtHQ(Matrix& dest) const
343  {
344  if (!m_computed)
345  throw std::logic_error("DoubleShiftQR: need to call compute() first");
346 
347  dest.noalias() = m_mat_H;
348  }
349 
350  // Q = P0 * P1 * ...
351  // Q'y = P_{n-2} * ... * P1 * P0 * y
352  void apply_QtY(Vector& y) const
353  {
354  if (!m_computed)
355  throw std::logic_error("DoubleShiftQR: need to call compute() first");
356 
357  Scalar* y_ptr = y.data();
358  const Index n1 = m_n - 1;
359  for (Index i = 0; i < n1; i++, y_ptr++)
360  {
361  apply_PX(y_ptr, i);
362  }
363  }
364 
365  // Q = P0 * P1 * ...
366  // YQ = Y * P0 * P1 * ...
367  void apply_YQ(GenericMatrix Y) const
368  {
369  if (!m_computed)
370  throw std::logic_error("DoubleShiftQR: need to call compute() first");
371 
372  const Index nrow = Y.rows();
373  const Index n2 = m_n - 2;
374  for (Index i = 0; i < n2; i++)
375  {
376  apply_XP(Y.block(0, i, nrow, 3), nrow, i);
377  }
378  apply_XP(Y.block(0, n2, nrow, 2), nrow, n2);
379  }
380 };
381 
382 } // namespace Spectra
383 
384 #endif // DOUBLE_SHIFT_QR_H
Eigen::Array< unsigned char, Eigen::Dynamic, 1 > IntArray
Definition: DoubleShiftQR.h:28
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: DoubleShiftQR.h:25
SCALAR Scalar
Definition: bench_gemm.cpp:46
const char Y
Scalar * y
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
#define min(a, b)
Definition: datatypes.h:19
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: DoubleShiftQR.h:27
const Scalar m_eps_abs
Definition: DoubleShiftQR.h:47
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
void compute(ConstGenericMatrix &mat, const Scalar &s, const Scalar &t)
Vector u2
void apply_YQ(GenericMatrix Y) const
void compute_reflector(const Scalar *x, Index ind)
Definition: DoubleShiftQR.h:94
void matrix_QtHQ(Matrix &dest) const
static double epsilon
Definition: testRot3.cpp:37
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar & coeffRef(Index rowId, Index colId)
void apply_XP(GenericMatrix X, Index stride, Index u_ind) const
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
void compute_reflector(const Scalar &x1, const Scalar &x2, const Scalar &x3, Index ind)
Definition: DoubleShiftQR.h:50
std::vector< int > ind
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 update_block(Index il, Index iu)
void apply_PX(GenericMatrix X, Index stride, Index u_ind) const
RealScalar s
static const double u0
Pose3 x3(Rot3::Ypr(M_PI/4.0, 0.0, 0.0), l2)
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:281
const double h
Eigen::Ref< Matrix > GenericMatrix
Definition: DoubleShiftQR.h:30
DoubleShiftQR(ConstGenericMatrix &mat, const Scalar &s, const Scalar &t)
Pose3 x1
Definition: testPose3.cpp:663
static EIGEN_DEPRECATED const end_t end
void apply_PX(Scalar *x, Index u_ind) const
const Eigen::Ref< const Matrix > ConstGenericMatrix
Definition: DoubleShiftQR.h:31
#define X
Definition: icosphere.cpp:20
const Scalar m_eps_rel
Definition: DoubleShiftQR.h:46
Eigen::Matrix< Scalar, 3, Eigen::Dynamic > Matrix3X
Definition: DoubleShiftQR.h:26
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
Point2 t(10, 10)
void apply_QtY(Vector &y) const


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:11