ImplicitQRSVD.h
1 
77 #ifndef JIXIE_IMPLICIT_QR_SVD_H
78 #define JIXIE_IMPLICIT_QR_SVD_H
79 
80 #include "Tools.h"
81 
82 namespace JIXIE {
83 
101 template <class T>
103 public:
104  int rowi;
105  int rowk;
106  T c;
107  T s;
108 
109  inline GivensRotation(int rowi_in, int rowk_in)
110  : rowi(rowi_in)
111  , rowk(rowk_in)
112  , c(1)
113  , s(0)
114  {
115  }
116 
117  inline GivensRotation(T a, T b, int rowi_in, int rowk_in)
118  : rowi(rowi_in)
119  , rowk(rowk_in)
120  {
121  compute(a, b);
122  }
123 
124  ~GivensRotation() {}
125 
126  inline void transposeInPlace()
127  {
128  s = -s;
129  }
130 
136  inline void compute(const T a, const T b)
137  {
138  using std::sqrt;
139 
140  T d = a * a + b * b;
141  c = 1;
142  s = 0;
143  if (d != 0) {
144  // T t = 1 / sqrt(d);
145  T t = JIXIE::MATH_TOOLS::rsqrt(d);
146  c = a * t;
147  s = -b * t;
148  }
149  }
150 
156  inline void computeUnconventional(const T a, const T b)
157  {
158  using std::sqrt;
159 
160  T d = a * a + b * b;
161  c = 0;
162  s = 1;
163  if (d != 0) {
164  // T t = 1 / sqrt(d);
165  T t = JIXIE::MATH_TOOLS::rsqrt(d);
166  s = a * t;
167  c = b * t;
168  }
169  }
173  template <class MatrixType>
174  inline void fill(const MatrixType& R) const
175  {
176  MatrixType& A = const_cast<MatrixType&>(R);
177  A = MatrixType::Identity();
178  A(rowi, rowi) = c;
179  A(rowk, rowi) = -s;
180  A(rowi, rowk) = s;
181  A(rowk, rowk) = c;
182  }
183 
191  template <class MatrixType>
192  inline void rowRotation(MatrixType& A) const
193  {
194  for (int j = 0; j < MatrixType::ColsAtCompileTime; j++) {
195  T tau1 = A(rowi, j);
196  T tau2 = A(rowk, j);
197  A(rowi, j) = c * tau1 - s * tau2;
198  A(rowk, j) = s * tau1 + c * tau2;
199  }
200  }
201 
209  template <class MatrixType>
210  inline void columnRotation(MatrixType& A) const
211  {
212  for (int j = 0; j < MatrixType::RowsAtCompileTime; j++) {
213  T tau1 = A(j, rowi);
214  T tau2 = A(j, rowk);
215  A(j, rowi) = c * tau1 - s * tau2;
216  A(j, rowk) = s * tau1 + c * tau2;
217  }
218  }
219 
223  inline void operator*=(const GivensRotation<T>& A)
224  {
225  T new_c = c * A.c - s * A.s;
226  T new_s = s * A.c + c * A.s;
227  c = new_c;
228  s = new_s;
229  }
230 
235  {
236  GivensRotation<T> r(*this);
237  r *= A;
238  return r;
239  }
240 };
241 
252 template <class T>
253 inline void zeroChase(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
254 {
255 
262  GivensRotation<T> r1(H(0, 0), H(1, 0), 0, 1);
271  GivensRotation<T> r2(1, 2);
272  if (H(1, 0) != 0)
273  r2.compute(H(0, 0) * H(0, 1) + H(1, 0) * H(1, 1), H(0, 0) * H(0, 2) + H(1, 0) * H(1, 2));
274  else
275  r2.compute(H(0, 1), H(0, 2));
276 
277  r1.rowRotation(H);
278 
279  /* GivensRotation<T> r2(H(0, 1), H(0, 2), 1, 2); */
280  r2.columnRotation(H);
281  r2.columnRotation(V);
282 
289  GivensRotation<T> r3(H(1, 1), H(2, 1), 1, 2);
290  r3.rowRotation(H);
291 
292  // Save this till end for better cache coherency
293  // r1.rowRotation(u_transpose);
294  // r3.rowRotation(u_transpose);
295  r1.columnRotation(U);
296  r3.columnRotation(U);
297 }
298 
309 template <class T>
310 inline void makeUpperBidiag(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
311 {
312  U = Eigen::Matrix<T, 3, 3>::Identity();
313  V = Eigen::Matrix<T, 3, 3>::Identity();
314 
322  GivensRotation<T> r(H(1, 0), H(2, 0), 1, 2);
323  r.rowRotation(H);
324  // r.rowRotation(u_transpose);
325  r.columnRotation(U);
326  // zeroChase(H, u_transpose, V);
327  zeroChase(H, U, V);
328 }
329 
340 template <class T>
341 inline void makeLambdaShape(Eigen::Matrix<T, 3, 3>& H, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 3>& V)
342 {
343  U = Eigen::Matrix<T, 3, 3>::Identity();
344  V = Eigen::Matrix<T, 3, 3>::Identity();
345 
353  GivensRotation<T> r1(H(0, 1), H(0, 2), 1, 2);
354  r1.columnRotation(H);
355  r1.columnRotation(V);
356 
364  r1.computeUnconventional(H(1, 2), H(2, 2));
365  r1.rowRotation(H);
366  r1.columnRotation(U);
367 
375  GivensRotation<T> r2(H(2, 0), H(2, 1), 0, 1);
376  r2.columnRotation(H);
377  r2.columnRotation(V);
378 
385  r2.computeUnconventional(H(0, 1), H(1, 1));
386  r2.rowRotation(H);
387  r2.columnRotation(U);
388 }
389 
401 template <class TA, class T, class TS>
402 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TS>(2, 2)>
403 polarDecomposition(const Eigen::MatrixBase<TA>& A,
405  const Eigen::MatrixBase<TS>& S_Sym)
406 {
407  Eigen::Matrix<T, 2, 1> x(A(0, 0) + A(1, 1), A(1, 0) - A(0, 1));
408  T denominator = x.norm();
409  R.c = (T)1;
410  R.s = (T)0;
411  if (denominator != 0) {
412  /*
413  No need to use a tolerance here because x(0) and x(1) always have
414  smaller magnitude then denominator, therefore overflow never happens.
415  */
416  R.c = x(0) / denominator;
417  R.s = -x(1) / denominator;
418  }
419  Eigen::MatrixBase<TS>& S = const_cast<Eigen::MatrixBase<TS>&>(S_Sym);
420  S = A;
421  R.rowRotation(S);
422 }
423 
435 template <class TA, class TR, class TS>
436 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TR>(2, 2) && isSize<TS>(2, 2)>
437 polarDecomposition(const Eigen::MatrixBase<TA>& A,
438  const Eigen::MatrixBase<TR>& R,
439  const Eigen::MatrixBase<TS>& S_Sym)
440 {
441  using T = ScalarType<TA>;
442  GivensRotation<T> r(0, 1);
443  polarDecomposition(A, r, S_Sym);
444  r.fill(R);
445 }
446 
454 template <class TA, class T, class Ts>
455 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<Ts>(2, 1)>
457  const Eigen::MatrixBase<TA>& A,
459  const Eigen::MatrixBase<Ts>& Sigma,
461  const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
462 {
463  using std::sqrt;
464  Eigen::MatrixBase<Ts>& sigma = const_cast<Eigen::MatrixBase<Ts>&>(Sigma);
465 
466  Eigen::Matrix<T, 2, 2> S_Sym;
467  polarDecomposition(A, U, S_Sym);
468  T cosine, sine;
469  T x = S_Sym(0, 0);
470  T y = S_Sym(0, 1);
471  T z = S_Sym(1, 1);
472  if (y == 0) {
473  // S is already diagonal
474  cosine = 1;
475  sine = 0;
476  sigma(0) = x;
477  sigma(1) = z;
478  }
479  else {
480  T tau = 0.5 * (x - z);
481  T w = sqrt(tau * tau + y * y);
482  // w > y > 0
483  T t;
484  if (tau > 0) {
485  // tau + w > w > y > 0 ==> division is safe
486  t = y / (tau + w);
487  }
488  else {
489  // tau - w < -w < -y < 0 ==> division is safe
490  t = y / (tau - w);
491  }
492  cosine = T(1) / sqrt(t * t + T(1));
493  sine = -t * cosine;
494  /*
495  V = [cosine -sine; sine cosine]
496  Sigma = V'SV. Only compute the diagonals for efficiency.
497  Also utilize symmetry of S and don't form V yet.
498  */
499  T c2 = cosine * cosine;
500  T csy = 2 * cosine * sine * y;
501  T s2 = sine * sine;
502  sigma(0) = c2 * x - csy + s2 * z;
503  sigma(1) = s2 * x + csy + c2 * z;
504  }
505 
506  // Sorting
507  // Polar already guarantees negative sign is on the small magnitude singular value.
508  if (sigma(0) < sigma(1)) {
509  std::swap(sigma(0), sigma(1));
510  V.c = -sine;
511  V.s = cosine;
512  }
513  else {
514  V.c = cosine;
515  V.s = sine;
516  }
517  U *= V;
518 }
526 template <class TA, class TU, class Ts, class TV>
527 inline std::enable_if_t<isSize<TA>(2, 2) && isSize<TU>(2, 2) && isSize<TV>(2, 2) && isSize<Ts>(2, 1)>
529  const Eigen::MatrixBase<TA>& A,
530  const Eigen::MatrixBase<TU>& U,
531  const Eigen::MatrixBase<Ts>& Sigma,
532  const Eigen::MatrixBase<TV>& V,
533  const ScalarType<TA> tol = 64 * std::numeric_limits<ScalarType<TA> >::epsilon())
534 {
535  using T = ScalarType<TA>;
536  GivensRotation<T> gv(0, 1);
537  GivensRotation<T> gu(0, 1);
538  singularValueDecomposition(A, gu, Sigma, gv);
539 
540  gu.fill(U);
541  gv.fill(V);
542 }
543 
552 template <class T>
553 T wilkinsonShift(const T a1, const T b1, const T a2)
554 {
555  using std::sqrt;
556  using std::fabs;
557  using std::copysign;
558 
559  T d = (T)0.5 * (a1 - a2);
560  T bs = b1 * b1;
561 
562  T mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d);
563  // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs));
564  return mu;
565 }
566 
570 template <int t, class T>
571 inline void process(Eigen::Matrix<T, 3, 3>& B, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
572 {
573  int other = (t == 1) ? 0 : 2;
574  GivensRotation<T> u(0, 1);
575  GivensRotation<T> v(0, 1);
576  sigma(other) = B(other, other);
577  singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v);
578  u.rowi += t;
579  u.rowk += t;
580  v.rowi += t;
581  v.rowk += t;
582  u.columnRotation(U);
583  v.columnRotation(V);
584 }
585 
589 template <class T>
590 inline void flipSign(int i, Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma)
591 {
592  sigma(i) = -sigma(i);
593  U.col(i) = -U.col(i);
594 }
595 
599 template <int t, class T>
600 std::enable_if_t<t == 0> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
601 {
602  using std::fabs;
603 
604  // Case: sigma(0) > |sigma(1)| >= |sigma(2)|
605  if (fabs(sigma(1)) >= fabs(sigma(2))) {
606  if (sigma(1) < 0) {
607  flipSign(1, U, sigma);
608  flipSign(2, U, sigma);
609  }
610  return;
611  }
612 
613  //fix sign of sigma for both cases
614  if (sigma(2) < 0) {
615  flipSign(1, U, sigma);
616  flipSign(2, U, sigma);
617  }
618 
619  //swap sigma(1) and sigma(2) for both cases
620  std::swap(sigma(1), sigma(2));
621  U.col(1).swap(U.col(2));
622  V.col(1).swap(V.col(2));
623 
624  // Case: |sigma(2)| >= sigma(0) > |simga(1)|
625  if (sigma(1) > sigma(0)) {
626  std::swap(sigma(0), sigma(1));
627  U.col(0).swap(U.col(1));
628  V.col(0).swap(V.col(1));
629  }
630 
631  // Case: sigma(0) >= |sigma(2)| > |simga(1)|
632  else {
633  U.col(2) = -U.col(2);
634  V.col(2) = -V.col(2);
635  }
636 }
637 
641 template <int t, class T>
642 std::enable_if_t<t == 1> sort(Eigen::Matrix<T, 3, 3>& U, Eigen::Matrix<T, 3, 1>& sigma, Eigen::Matrix<T, 3, 3>& V)
643 {
644  using std::fabs;
645 
646  // Case: |sigma(0)| >= sigma(1) > |sigma(2)|
647  if (fabs(sigma(0)) >= sigma(1)) {
648  if (sigma(0) < 0) {
649  flipSign(0, U, sigma);
650  flipSign(2, U, sigma);
651  }
652  return;
653  }
654 
655  //swap sigma(0) and sigma(1) for both cases
656  std::swap(sigma(0), sigma(1));
657  U.col(0).swap(U.col(1));
658  V.col(0).swap(V.col(1));
659 
660  // Case: sigma(1) > |sigma(2)| >= |sigma(0)|
661  if (fabs(sigma(1)) < fabs(sigma(2))) {
662  std::swap(sigma(1), sigma(2));
663  U.col(1).swap(U.col(2));
664  V.col(1).swap(V.col(2));
665  }
666 
667  // Case: sigma(1) >= |sigma(0)| > |sigma(2)|
668  else {
669  U.col(1) = -U.col(1);
670  V.col(1) = -V.col(1);
671  }
672 
673  // fix sign for both cases
674  if (sigma(1) < 0) {
675  flipSign(1, U, sigma);
676  flipSign(2, U, sigma);
677  }
678 }
679 
687 template <class T>
688 inline int singularValueDecomposition(const Eigen::Matrix<T, 3, 3>& A,
689  Eigen::Matrix<T, 3, 3>& U,
690  Eigen::Matrix<T, 3, 1>& sigma,
691  Eigen::Matrix<T, 3, 3>& V,
692  T tol = 1024 * std::numeric_limits<T>::epsilon())
693 {
694  using std::fabs;
695  using std::sqrt;
696  using std::max;
697  Eigen::Matrix<T, 3, 3> B = A;
698  U = Eigen::Matrix<T, 3, 3>::Identity();
699  V = Eigen::Matrix<T, 3, 3>::Identity();
700 
701  makeUpperBidiag(B, U, V);
702 
703  int count = 0;
704  T mu = (T)0;
705  GivensRotation<T> r(0, 1);
706 
707  T alpha_1 = B(0, 0);
708  T beta_1 = B(0, 1);
709  T alpha_2 = B(1, 1);
710  T alpha_3 = B(2, 2);
711  T beta_2 = B(1, 2);
712  T gamma_1 = alpha_1 * beta_1;
713  T gamma_2 = alpha_2 * beta_2;
714  tol *= max((T)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (T)1);
715 
720  while (fabs(beta_2) > tol && fabs(beta_1) > tol
721  && fabs(alpha_1) > tol && fabs(alpha_2) > tol
722  && fabs(alpha_3) > tol) {
723  mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2);
724 
725  r.compute(alpha_1 * alpha_1 - mu, gamma_1);
726  r.columnRotation(B);
727 
728  r.columnRotation(V);
729  zeroChase(B, U, V);
730 
731  alpha_1 = B(0, 0);
732  beta_1 = B(0, 1);
733  alpha_2 = B(1, 1);
734  alpha_3 = B(2, 2);
735  beta_2 = B(1, 2);
736  gamma_1 = alpha_1 * beta_1;
737  gamma_2 = alpha_2 * beta_2;
738  count++;
739  }
750  if (fabs(beta_2) <= tol) {
751  process<0>(B, U, sigma, V);
752  sort<0>(U, sigma, V);
753  }
760  else if (fabs(beta_1) <= tol) {
761  process<1>(B, U, sigma, V);
762  sort<1>(U, sigma, V);
763  }
770  else if (fabs(alpha_2) <= tol) {
777  GivensRotation<T> r1(1, 2);
778  r1.computeUnconventional(B(1, 2), B(2, 2));
779  r1.rowRotation(B);
780  r1.columnRotation(U);
781 
782  process<0>(B, U, sigma, V);
783  sort<0>(U, sigma, V);
784  }
791  else if (fabs(alpha_3) <= tol) {
798  GivensRotation<T> r1(1, 2);
799  r1.compute(B(1, 1), B(1, 2));
800  r1.columnRotation(B);
801  r1.columnRotation(V);
808  GivensRotation<T> r2(0, 2);
809  r2.compute(B(0, 0), B(0, 2));
810  r2.columnRotation(B);
811  r2.columnRotation(V);
812 
813  process<0>(B, U, sigma, V);
814  sort<0>(U, sigma, V);
815  }
822  else if (fabs(alpha_1) <= tol) {
829  GivensRotation<T> r1(0, 1);
830  r1.computeUnconventional(B(0, 1), B(1, 1));
831  r1.rowRotation(B);
832  r1.columnRotation(U);
833 
840  GivensRotation<T> r2(0, 2);
841  r2.computeUnconventional(B(0, 2), B(2, 2));
842  r2.rowRotation(B);
843  r2.columnRotation(U);
844 
845  process<1>(B, U, sigma, V);
846  sort<1>(U, sigma, V);
847  }
848 
849  return count;
850 }
851 
863 template <class T>
864 inline void polarDecomposition(const Eigen::Matrix<T, 3, 3>& A,
865  Eigen::Matrix<T, 3, 3>& R,
866  Eigen::Matrix<T, 3, 3>& S_Sym)
867 {
868  Eigen::Matrix<T, 3, 3> U;
869  Eigen::Matrix<T, 3, 1> sigma;
870  Eigen::Matrix<T, 3, 3> V;
871 
872  singularValueDecomposition(A, U, sigma, V);
873  R.noalias() = U * V.transpose();
874  S_Sym.noalias() = V * Eigen::DiagonalMatrix<T, 3, 3>(sigma) * V.transpose();
875 }
876 }
877 #endif