Teh first one
[mldemos:kalians-mldemos.git] / _AlgorithmsPlugins / KPCA / Eigen / src / Eigen2Support / SVD.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra. Eigen itself is part of the KDE project.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24
25 #ifndef EIGEN2_SVD_H
26 #define EIGEN2_SVD_H
27
28 /** \ingroup SVD_Module
29   * \nonstableyet
30   *
31   * \class SVD
32   *
33   * \brief Standard SVD decomposition of a matrix and associated features
34   *
35   * \param MatrixType the type of the matrix of which we are computing the SVD decomposition
36   *
37   * This class performs a standard SVD decomposition of a real matrix A of size \c M x \c N
38   * with \c M \>= \c N.
39   *
40   *
41   * \sa MatrixBase::SVD()
42   */
43 template<typename MatrixType> class SVD
44 {
45   private:
46     typedef typename MatrixType::Scalar Scalar;
47     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
48
49     enum {
50       PacketSize = internal::packet_traits<Scalar>::size,
51       AlignmentMask = int(PacketSize)-1,
52       MinSize = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)
53     };
54
55     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
56     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
57
58     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
59     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
60     typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
61
62   public:
63
64     SVD() {} // a user who relied on compiler-generated default compiler reported problems with MSVC in 2.0.7
65     
66     SVD(const MatrixType& matrix)
67       : m_matU(matrix.rows(), std::min(matrix.rows(), matrix.cols())),
68         m_matV(matrix.cols(),matrix.cols()),
69         m_sigma(std::min(matrix.rows(),matrix.cols()))
70     {
71       compute(matrix);
72     }
73
74     template<typename OtherDerived, typename ResultType>
75     bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
76
77     const MatrixUType& matrixU() const { return m_matU; }
78     const SingularValuesType& singularValues() const { return m_sigma; }
79     const MatrixVType& matrixV() const { return m_matV; }
80
81     void compute(const MatrixType& matrix);
82     SVD& sort();
83
84     template<typename UnitaryType, typename PositiveType>
85     void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const;
86     template<typename PositiveType, typename UnitaryType>
87     void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const;
88     template<typename RotationType, typename ScalingType>
89     void computeRotationScaling(RotationType *unitary, ScalingType *positive) const;
90     template<typename ScalingType, typename RotationType>
91     void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
92
93   protected:
94     /** \internal */
95     MatrixUType m_matU;
96     /** \internal */
97     MatrixVType m_matV;
98     /** \internal */
99     SingularValuesType m_sigma;
100 };
101
102 /** Computes / recomputes the SVD decomposition A = U S V^* of \a matrix
103   *
104   * \note this code has been adapted from JAMA (public domain)
105   */
106 template<typename MatrixType>
107 void SVD<MatrixType>::compute(const MatrixType& matrix)
108 {
109   const int m = matrix.rows();
110   const int n = matrix.cols();
111   const int nu = std::min(m,n);
112   ei_assert(m>=n && "In Eigen 2.0, SVD only works for MxN matrices with M>=N. Sorry!");
113   ei_assert(m>1 && "In Eigen 2.0, SVD doesn't work on 1x1 matrices");
114
115   m_matU.resize(m, nu);
116   m_matU.setZero();
117   m_sigma.resize(std::min(m,n));
118   m_matV.resize(n,n);
119
120   RowVector e(n);
121   ColVector work(m);
122   MatrixType matA(matrix);
123   const bool wantu = true;
124   const bool wantv = true;
125   int i=0, j=0, k=0;
126
127   // Reduce A to bidiagonal form, storing the diagonal elements
128   // in s and the super-diagonal elements in e.
129   int nct = std::min(m-1,n);
130   int nrt = std::max(0,std::min(n-2,m));
131   for (k = 0; k < std::max(nct,nrt); ++k)
132   {
133     if (k < nct)
134     {
135       // Compute the transformation for the k-th column and
136       // place the k-th diagonal in m_sigma[k].
137       m_sigma[k] = matA.col(k).end(m-k).norm();
138       if (m_sigma[k] != 0.0) // FIXME
139       {
140         if (matA(k,k) < 0.0)
141           m_sigma[k] = -m_sigma[k];
142         matA.col(k).end(m-k) /= m_sigma[k];
143         matA(k,k) += 1.0;
144       }
145       m_sigma[k] = -m_sigma[k];
146     }
147
148     for (j = k+1; j < n; ++j)
149     {
150       if ((k < nct) && (m_sigma[k] != 0.0))
151       {
152         // Apply the transformation.
153         Scalar t = matA.col(k).end(m-k).eigen2_dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
154         t = -t/matA(k,k);
155         matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
156       }
157
158       // Place the k-th row of A into e for the
159       // subsequent calculation of the row transformation.
160       e[j] = matA(k,j);
161     }
162
163     // Place the transformation in U for subsequent back multiplication.
164     if (wantu & (k < nct))
165       m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
166
167     if (k < nrt)
168     {
169       // Compute the k-th row transformation and place the
170       // k-th super-diagonal in e[k].
171       e[k] = e.end(n-k-1).norm();
172       if (e[k] != 0.0)
173       {
174           if (e[k+1] < 0.0)
175             e[k] = -e[k];
176           e.end(n-k-1) /= e[k];
177           e[k+1] += 1.0;
178       }
179       e[k] = -e[k];
180       if ((k+1 < m) & (e[k] != 0.0))
181       {
182         // Apply the transformation.
183         work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
184         for (j = k+1; j < n; ++j)
185           matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
186       }
187
188       // Place the transformation in V for subsequent back multiplication.
189       if (wantv)
190         m_matV.col(k).end(n-k-1) = e.end(n-k-1);
191     }
192   }
193
194
195   // Set up the final bidiagonal matrix or order p.
196   int p = std::min(n,m+1);
197   if (nct < n)
198     m_sigma[nct] = matA(nct,nct);
199   if (m < p)
200     m_sigma[p-1] = 0.0;
201   if (nrt+1 < p)
202     e[nrt] = matA(nrt,p-1);
203   e[p-1] = 0.0;
204
205   // If required, generate U.
206   if (wantu)
207   {
208     for (j = nct; j < nu; ++j)
209     {
210       m_matU.col(j).setZero();
211       m_matU(j,j) = 1.0;
212     }
213     for (k = nct-1; k >= 0; k--)
214     {
215       if (m_sigma[k] != 0.0)
216       {
217         for (j = k+1; j < nu; ++j)
218         {
219           Scalar t = m_matU.col(k).end(m-k).eigen2_dot(m_matU.col(j).end(m-k)); // FIXME is it really a dot product we want ?
220           t = -t/m_matU(k,k);
221           m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
222         }
223         m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
224         m_matU(k,k) = Scalar(1) + m_matU(k,k);
225         if (k-1>0)
226           m_matU.col(k).start(k-1).setZero();
227       }
228       else
229       {
230         m_matU.col(k).setZero();
231         m_matU(k,k) = 1.0;
232       }
233     }
234   }
235
236   // If required, generate V.
237   if (wantv)
238   {
239     for (k = n-1; k >= 0; k--)
240     {
241       if ((k < nrt) & (e[k] != 0.0))
242       {
243         for (j = k+1; j < nu; ++j)
244         {
245           Scalar t = m_matV.col(k).end(n-k-1).eigen2_dot(m_matV.col(j).end(n-k-1)); // FIXME is it really a dot product we want ?
246           t = -t/m_matV(k+1,k);
247           m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
248         }
249       }
250       m_matV.col(k).setZero();
251       m_matV(k,k) = 1.0;
252     }
253   }
254
255   // Main iteration loop for the singular values.
256   int pp = p-1;
257   int iter = 0;
258   Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
259   while (p > 0)
260   {
261     int k=0;
262     int kase=0;
263
264     // Here is where a test for too many iterations would go.
265
266     // This section of the program inspects for
267     // negligible elements in the s and e arrays.  On
268     // completion the variables kase and k are set as follows.
269
270     // kase = 1     if s(p) and e[k-1] are negligible and k<p
271     // kase = 2     if s(k) is negligible and k<p
272     // kase = 3     if e[k-1] is negligible, k<p, and
273     //              s(k), ..., s(p) are not negligible (qr step).
274     // kase = 4     if e(p-1) is negligible (convergence).
275
276     for (k = p-2; k >= -1; --k)
277     {
278       if (k == -1)
279           break;
280       if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
281       {
282           e[k] = 0.0;
283           break;
284       }
285     }
286     if (k == p-2)
287     {
288       kase = 4;
289     }
290     else
291     {
292       int ks;
293       for (ks = p-1; ks >= k; --ks)
294       {
295         if (ks == k)
296           break;
297         Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
298         if (ei_abs(m_sigma[ks]) <= eps*t)
299         {
300           m_sigma[ks] = 0.0;
301           break;
302         }
303       }
304       if (ks == k)
305       {
306         kase = 3;
307       }
308       else if (ks == p-1)
309       {
310         kase = 1;
311       }
312       else
313       {
314         kase = 2;
315         k = ks;
316       }
317     }
318     ++k;
319
320     // Perform the task indicated by kase.
321     switch (kase)
322     {
323
324       // Deflate negligible s(p).
325       case 1:
326       {
327         Scalar f(e[p-2]);
328         e[p-2] = 0.0;
329         for (j = p-2; j >= k; --j)
330         {
331           Scalar t(internal::hypot(m_sigma[j],f));
332           Scalar cs(m_sigma[j]/t);
333           Scalar sn(f/t);
334           m_sigma[j] = t;
335           if (j != k)
336           {
337             f = -sn*e[j-1];
338             e[j-1] = cs*e[j-1];
339           }
340           if (wantv)
341           {
342             for (i = 0; i < n; ++i)
343             {
344               t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
345               m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
346               m_matV(i,j) = t;
347             }
348           }
349         }
350       }
351       break;
352
353       // Split at negligible s(k).
354       case 2:
355       {
356         Scalar f(e[k-1]);
357         e[k-1] = 0.0;
358         for (j = k; j < p; ++j)
359         {
360           Scalar t(internal::hypot(m_sigma[j],f));
361           Scalar cs( m_sigma[j]/t);
362           Scalar sn(f/t);
363           m_sigma[j] = t;
364           f = -sn*e[j];
365           e[j] = cs*e[j];
366           if (wantu)
367           {
368             for (i = 0; i < m; ++i)
369             {
370               t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
371               m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
372               m_matU(i,j) = t;
373             }
374           }
375         }
376       }
377       break;
378
379       // Perform one qr step.
380       case 3:
381       {
382         // Calculate the shift.
383         Scalar scale = std::max(std::max(std::max(std::max(
384                         ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
385                         ei_abs(m_sigma[k])),ei_abs(e[k]));
386         Scalar sp = m_sigma[p-1]/scale;
387         Scalar spm1 = m_sigma[p-2]/scale;
388         Scalar epm1 = e[p-2]/scale;
389         Scalar sk = m_sigma[k]/scale;
390         Scalar ek = e[k]/scale;
391         Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
392         Scalar c = (sp*epm1)*(sp*epm1);
393         Scalar shift = 0.0;
394         if ((b != 0.0) || (c != 0.0))
395         {
396           shift = ei_sqrt(b*b + c);
397           if (b < 0.0)
398             shift = -shift;
399           shift = c/(b + shift);
400         }
401         Scalar f = (sk + sp)*(sk - sp) + shift;
402         Scalar g = sk*ek;
403
404         // Chase zeros.
405
406         for (j = k; j < p-1; ++j)
407         {
408           Scalar t = internal::hypot(f,g);
409           Scalar cs = f/t;
410           Scalar sn = g/t;
411           if (j != k)
412             e[j-1] = t;
413           f = cs*m_sigma[j] + sn*e[j];
414           e[j] = cs*e[j] - sn*m_sigma[j];
415           g = sn*m_sigma[j+1];
416           m_sigma[j+1] = cs*m_sigma[j+1];
417           if (wantv)
418           {
419             for (i = 0; i < n; ++i)
420             {
421               t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
422               m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
423               m_matV(i,j) = t;
424             }
425           }
426           t = internal::hypot(f,g);
427           cs = f/t;
428           sn = g/t;
429           m_sigma[j] = t;
430           f = cs*e[j] + sn*m_sigma[j+1];
431           m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
432           g = sn*e[j+1];
433           e[j+1] = cs*e[j+1];
434           if (wantu && (j < m-1))
435           {
436             for (i = 0; i < m; ++i)
437             {
438               t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
439               m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
440               m_matU(i,j) = t;
441             }
442           }
443         }
444         e[p-2] = f;
445         iter = iter + 1;
446       }
447       break;
448
449       // Convergence.
450       case 4:
451       {
452         // Make the singular values positive.
453         if (m_sigma[k] <= 0.0)
454         {
455           m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
456           if (wantv)
457             m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
458         }
459
460         // Order the singular values.
461         while (k < pp)
462         {
463           if (m_sigma[k] >= m_sigma[k+1])
464             break;
465           Scalar t = m_sigma[k];
466           m_sigma[k] = m_sigma[k+1];
467           m_sigma[k+1] = t;
468           if (wantv && (k < n-1))
469             m_matV.col(k).swap(m_matV.col(k+1));
470           if (wantu && (k < m-1))
471             m_matU.col(k).swap(m_matU.col(k+1));
472           ++k;
473         }
474         iter = 0;
475         p--;
476       }
477       break;
478     } // end big switch
479   } // end iterations
480 }
481
482 template<typename MatrixType>
483 SVD<MatrixType>& SVD<MatrixType>::sort()
484 {
485   int mu = m_matU.rows();
486   int mv = m_matV.rows();
487   int n  = m_matU.cols();
488
489   for (int i=0; i<n; ++i)
490   {
491     int  k = i;
492     Scalar p = m_sigma.coeff(i);
493
494     for (int j=i+1; j<n; ++j)
495     {
496       if (m_sigma.coeff(j) > p)
497       {
498         k = j;
499         p = m_sigma.coeff(j);
500       }
501     }
502     if (k != i)
503     {
504       m_sigma.coeffRef(k) = m_sigma.coeff(i);  // i.e.
505       m_sigma.coeffRef(i) = p;                 // swaps the i-th and the k-th elements
506
507       int j = mu;
508       for(int s=0; j!=0; ++s, --j)
509         std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k));
510
511       j = mv;
512       for (int s=0; j!=0; ++s, --j)
513         std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k));
514     }
515   }
516   return *this;
517 }
518
519 /** \returns the solution of \f$ A x = b \f$ using the current SVD decomposition of A.
520   * The parts of the solution corresponding to zero singular values are ignored.
521   *
522   * \sa MatrixBase::svd(), LU::solve(), LLT::solve()
523   */
524 template<typename MatrixType>
525 template<typename OtherDerived, typename ResultType>
526 bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
527 {
528   const int rows = m_matU.rows();
529   ei_assert(b.rows() == rows);
530
531   Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
532   for (int j=0; j<b.cols(); ++j)
533   {
534     Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j);
535
536     for (int i = 0; i <m_matU.cols(); ++i)
537     {
538       Scalar si = m_sigma.coeff(i);
539       if (ei_isMuchSmallerThan(ei_abs(si),maxVal))
540         aux.coeffRef(i) = 0;
541       else
542         aux.coeffRef(i) /= si;
543     }
544
545     result->col(j) = m_matV * aux;
546   }
547   return true;
548 }
549
550 /** Computes the polar decomposition of the matrix, as a product unitary x positive.
551   *
552   * If either pointer is zero, the corresponding computation is skipped.
553   *
554   * Only for square matrices.
555   *
556   * \sa computePositiveUnitary(), computeRotationScaling()
557   */
558 template<typename MatrixType>
559 template<typename UnitaryType, typename PositiveType>
560 void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary,
561                                              PositiveType *positive) const
562 {
563   ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices");
564   if(unitary) *unitary = m_matU * m_matV.adjoint();
565   if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint();
566 }
567
568 /** Computes the polar decomposition of the matrix, as a product positive x unitary.
569   *
570   * If either pointer is zero, the corresponding computation is skipped.
571   *
572   * Only for square matrices.
573   *
574   * \sa computeUnitaryPositive(), computeRotationScaling()
575   */
576 template<typename MatrixType>
577 template<typename UnitaryType, typename PositiveType>
578 void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive,
579                                              PositiveType *unitary) const
580 {
581   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
582   if(unitary) *unitary = m_matU * m_matV.adjoint();
583   if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint();
584 }
585
586 /** decomposes the matrix as a product rotation x scaling, the scaling being
587   * not necessarily positive.
588   *
589   * If either pointer is zero, the corresponding computation is skipped.
590   *
591   * This method requires the Geometry module.
592   *
593   * \sa computeScalingRotation(), computeUnitaryPositive()
594   */
595 template<typename MatrixType>
596 template<typename RotationType, typename ScalingType>
597 void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const
598 {
599   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
600   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
601   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
602   sv.coeffRef(0) *= x;
603   if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint());
604   if(rotation)
605   {
606     MatrixType m(m_matU);
607     m.col(0) /= x;
608     rotation->lazyAssign(m * m_matV.adjoint());
609   }
610 }
611
612 /** decomposes the matrix as a product scaling x rotation, the scaling being
613   * not necessarily positive.
614   *
615   * If either pointer is zero, the corresponding computation is skipped.
616   *
617   * This method requires the Geometry module.
618   *
619   * \sa computeRotationScaling(), computeUnitaryPositive()
620   */
621 template<typename MatrixType>
622 template<typename ScalingType, typename RotationType>
623 void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const
624 {
625   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
626   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
627   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
628   sv.coeffRef(0) *= x;
629   if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint());
630   if(rotation)
631   {
632     MatrixType m(m_matU);
633     m.col(0) /= x;
634     rotation->lazyAssign(m * m_matV.adjoint());
635   }
636 }
637
638
639 /** \svd_module
640   * \returns the SVD decomposition of \c *this
641   */
642 template<typename Derived>
643 inline SVD<typename MatrixBase<Derived>::PlainObject>
644 MatrixBase<Derived>::svd() const
645 {
646   return SVD<PlainObject>(derived());
647 }
648
649 #endif // EIGEN2_SVD_H