SVD.h
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
00005 //
00006 // This Source Code Form is subject to the terms of the Mozilla
00007 // Public License v. 2.0. If a copy of the MPL was not distributed
00008 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00009 
00010 #ifndef EIGEN2_SVD_H
00011 #define EIGEN2_SVD_H
00012 
00013 namespace Eigen {
00014 
00030 template<typename MatrixType> class SVD
00031 {
00032   private:
00033     typedef typename MatrixType::Scalar Scalar;
00034     typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
00035 
00036     enum {
00037       PacketSize = internal::packet_traits<Scalar>::size,
00038       AlignmentMask = int(PacketSize)-1,
00039       MinSize = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime)
00040     };
00041 
00042     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> ColVector;
00043     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> RowVector;
00044 
00045     typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, MinSize> MatrixUType;
00046     typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> MatrixVType;
00047     typedef Matrix<Scalar, MinSize, 1> SingularValuesType;
00048 
00049   public:
00050 
00051     SVD() {} // a user who relied on compiler-generated default compiler reported problems with MSVC in 2.0.7
00052     
00053     SVD(const MatrixType& matrix)
00054       : m_matU(matrix.rows(), (std::min)(matrix.rows(), matrix.cols())),
00055         m_matV(matrix.cols(),matrix.cols()),
00056         m_sigma((std::min)(matrix.rows(),matrix.cols()))
00057     {
00058       compute(matrix);
00059     }
00060 
00061     template<typename OtherDerived, typename ResultType>
00062     bool solve(const MatrixBase<OtherDerived> &b, ResultType* result) const;
00063 
00064     const MatrixUType& matrixU() const { return m_matU; }
00065     const SingularValuesType& singularValues() const { return m_sigma; }
00066     const MatrixVType& matrixV() const { return m_matV; }
00067 
00068     void compute(const MatrixType& matrix);
00069     SVD& sort();
00070 
00071     template<typename UnitaryType, typename PositiveType>
00072     void computeUnitaryPositive(UnitaryType *unitary, PositiveType *positive) const;
00073     template<typename PositiveType, typename UnitaryType>
00074     void computePositiveUnitary(PositiveType *positive, UnitaryType *unitary) const;
00075     template<typename RotationType, typename ScalingType>
00076     void computeRotationScaling(RotationType *unitary, ScalingType *positive) const;
00077     template<typename ScalingType, typename RotationType>
00078     void computeScalingRotation(ScalingType *positive, RotationType *unitary) const;
00079 
00080   protected:
00082     MatrixUType m_matU;
00084     MatrixVType m_matV;
00086     SingularValuesType m_sigma;
00087 };
00088 
00093 template<typename MatrixType>
00094 void SVD<MatrixType>::compute(const MatrixType& matrix)
00095 {
00096   const int m = matrix.rows();
00097   const int n = matrix.cols();
00098   const int nu = (std::min)(m,n);
00099   ei_assert(m>=n && "In Eigen 2.0, SVD only works for MxN matrices with M>=N. Sorry!");
00100   ei_assert(m>1 && "In Eigen 2.0, SVD doesn't work on 1x1 matrices");
00101 
00102   m_matU.resize(m, nu);
00103   m_matU.setZero();
00104   m_sigma.resize((std::min)(m,n));
00105   m_matV.resize(n,n);
00106 
00107   RowVector e(n);
00108   ColVector work(m);
00109   MatrixType matA(matrix);
00110   const bool wantu = true;
00111   const bool wantv = true;
00112   int i=0, j=0, k=0;
00113 
00114   // Reduce A to bidiagonal form, storing the diagonal elements
00115   // in s and the super-diagonal elements in e.
00116   int nct = (std::min)(m-1,n);
00117   int nrt = (std::max)(0,(std::min)(n-2,m));
00118   for (k = 0; k < (std::max)(nct,nrt); ++k)
00119   {
00120     if (k < nct)
00121     {
00122       // Compute the transformation for the k-th column and
00123       // place the k-th diagonal in m_sigma[k].
00124       m_sigma[k] = matA.col(k).end(m-k).norm();
00125       if (m_sigma[k] != 0.0) // FIXME
00126       {
00127         if (matA(k,k) < 0.0)
00128           m_sigma[k] = -m_sigma[k];
00129         matA.col(k).end(m-k) /= m_sigma[k];
00130         matA(k,k) += 1.0;
00131       }
00132       m_sigma[k] = -m_sigma[k];
00133     }
00134 
00135     for (j = k+1; j < n; ++j)
00136     {
00137       if ((k < nct) && (m_sigma[k] != 0.0))
00138       {
00139         // Apply the transformation.
00140         Scalar t = matA.col(k).end(m-k).eigen2_dot(matA.col(j).end(m-k)); // FIXME dot product or cwise prod + .sum() ??
00141         t = -t/matA(k,k);
00142         matA.col(j).end(m-k) += t * matA.col(k).end(m-k);
00143       }
00144 
00145       // Place the k-th row of A into e for the
00146       // subsequent calculation of the row transformation.
00147       e[j] = matA(k,j);
00148     }
00149 
00150     // Place the transformation in U for subsequent back multiplication.
00151     if (wantu & (k < nct))
00152       m_matU.col(k).end(m-k) = matA.col(k).end(m-k);
00153 
00154     if (k < nrt)
00155     {
00156       // Compute the k-th row transformation and place the
00157       // k-th super-diagonal in e[k].
00158       e[k] = e.end(n-k-1).norm();
00159       if (e[k] != 0.0)
00160       {
00161           if (e[k+1] < 0.0)
00162             e[k] = -e[k];
00163           e.end(n-k-1) /= e[k];
00164           e[k+1] += 1.0;
00165       }
00166       e[k] = -e[k];
00167       if ((k+1 < m) & (e[k] != 0.0))
00168       {
00169         // Apply the transformation.
00170         work.end(m-k-1) = matA.corner(BottomRight,m-k-1,n-k-1) * e.end(n-k-1);
00171         for (j = k+1; j < n; ++j)
00172           matA.col(j).end(m-k-1) += (-e[j]/e[k+1]) * work.end(m-k-1);
00173       }
00174 
00175       // Place the transformation in V for subsequent back multiplication.
00176       if (wantv)
00177         m_matV.col(k).end(n-k-1) = e.end(n-k-1);
00178     }
00179   }
00180 
00181 
00182   // Set up the final bidiagonal matrix or order p.
00183   int p = (std::min)(n,m+1);
00184   if (nct < n)
00185     m_sigma[nct] = matA(nct,nct);
00186   if (m < p)
00187     m_sigma[p-1] = 0.0;
00188   if (nrt+1 < p)
00189     e[nrt] = matA(nrt,p-1);
00190   e[p-1] = 0.0;
00191 
00192   // If required, generate U.
00193   if (wantu)
00194   {
00195     for (j = nct; j < nu; ++j)
00196     {
00197       m_matU.col(j).setZero();
00198       m_matU(j,j) = 1.0;
00199     }
00200     for (k = nct-1; k >= 0; k--)
00201     {
00202       if (m_sigma[k] != 0.0)
00203       {
00204         for (j = k+1; j < nu; ++j)
00205         {
00206           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 ?
00207           t = -t/m_matU(k,k);
00208           m_matU.col(j).end(m-k) += t * m_matU.col(k).end(m-k);
00209         }
00210         m_matU.col(k).end(m-k) = - m_matU.col(k).end(m-k);
00211         m_matU(k,k) = Scalar(1) + m_matU(k,k);
00212         if (k-1>0)
00213           m_matU.col(k).start(k-1).setZero();
00214       }
00215       else
00216       {
00217         m_matU.col(k).setZero();
00218         m_matU(k,k) = 1.0;
00219       }
00220     }
00221   }
00222 
00223   // If required, generate V.
00224   if (wantv)
00225   {
00226     for (k = n-1; k >= 0; k--)
00227     {
00228       if ((k < nrt) & (e[k] != 0.0))
00229       {
00230         for (j = k+1; j < nu; ++j)
00231         {
00232           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 ?
00233           t = -t/m_matV(k+1,k);
00234           m_matV.col(j).end(n-k-1) += t * m_matV.col(k).end(n-k-1);
00235         }
00236       }
00237       m_matV.col(k).setZero();
00238       m_matV(k,k) = 1.0;
00239     }
00240   }
00241 
00242   // Main iteration loop for the singular values.
00243   int pp = p-1;
00244   int iter = 0;
00245   Scalar eps = ei_pow(Scalar(2),ei_is_same_type<Scalar,float>::ret ? Scalar(-23) : Scalar(-52));
00246   while (p > 0)
00247   {
00248     int k=0;
00249     int kase=0;
00250 
00251     // Here is where a test for too many iterations would go.
00252 
00253     // This section of the program inspects for
00254     // negligible elements in the s and e arrays.  On
00255     // completion the variables kase and k are set as follows.
00256 
00257     // kase = 1     if s(p) and e[k-1] are negligible and k<p
00258     // kase = 2     if s(k) is negligible and k<p
00259     // kase = 3     if e[k-1] is negligible, k<p, and
00260     //              s(k), ..., s(p) are not negligible (qr step).
00261     // kase = 4     if e(p-1) is negligible (convergence).
00262 
00263     for (k = p-2; k >= -1; --k)
00264     {
00265       if (k == -1)
00266           break;
00267       if (ei_abs(e[k]) <= eps*(ei_abs(m_sigma[k]) + ei_abs(m_sigma[k+1])))
00268       {
00269           e[k] = 0.0;
00270           break;
00271       }
00272     }
00273     if (k == p-2)
00274     {
00275       kase = 4;
00276     }
00277     else
00278     {
00279       int ks;
00280       for (ks = p-1; ks >= k; --ks)
00281       {
00282         if (ks == k)
00283           break;
00284         Scalar t = (ks != p ? ei_abs(e[ks]) : Scalar(0)) + (ks != k+1 ? ei_abs(e[ks-1]) : Scalar(0));
00285         if (ei_abs(m_sigma[ks]) <= eps*t)
00286         {
00287           m_sigma[ks] = 0.0;
00288           break;
00289         }
00290       }
00291       if (ks == k)
00292       {
00293         kase = 3;
00294       }
00295       else if (ks == p-1)
00296       {
00297         kase = 1;
00298       }
00299       else
00300       {
00301         kase = 2;
00302         k = ks;
00303       }
00304     }
00305     ++k;
00306 
00307     // Perform the task indicated by kase.
00308     switch (kase)
00309     {
00310 
00311       // Deflate negligible s(p).
00312       case 1:
00313       {
00314         Scalar f(e[p-2]);
00315         e[p-2] = 0.0;
00316         for (j = p-2; j >= k; --j)
00317         {
00318           Scalar t(numext::hypot(m_sigma[j],f));
00319           Scalar cs(m_sigma[j]/t);
00320           Scalar sn(f/t);
00321           m_sigma[j] = t;
00322           if (j != k)
00323           {
00324             f = -sn*e[j-1];
00325             e[j-1] = cs*e[j-1];
00326           }
00327           if (wantv)
00328           {
00329             for (i = 0; i < n; ++i)
00330             {
00331               t = cs*m_matV(i,j) + sn*m_matV(i,p-1);
00332               m_matV(i,p-1) = -sn*m_matV(i,j) + cs*m_matV(i,p-1);
00333               m_matV(i,j) = t;
00334             }
00335           }
00336         }
00337       }
00338       break;
00339 
00340       // Split at negligible s(k).
00341       case 2:
00342       {
00343         Scalar f(e[k-1]);
00344         e[k-1] = 0.0;
00345         for (j = k; j < p; ++j)
00346         {
00347           Scalar t(numext::hypot(m_sigma[j],f));
00348           Scalar cs( m_sigma[j]/t);
00349           Scalar sn(f/t);
00350           m_sigma[j] = t;
00351           f = -sn*e[j];
00352           e[j] = cs*e[j];
00353           if (wantu)
00354           {
00355             for (i = 0; i < m; ++i)
00356             {
00357               t = cs*m_matU(i,j) + sn*m_matU(i,k-1);
00358               m_matU(i,k-1) = -sn*m_matU(i,j) + cs*m_matU(i,k-1);
00359               m_matU(i,j) = t;
00360             }
00361           }
00362         }
00363       }
00364       break;
00365 
00366       // Perform one qr step.
00367       case 3:
00368       {
00369         // Calculate the shift.
00370         Scalar scale = (std::max)((std::max)((std::max)((std::max)(
00371                         ei_abs(m_sigma[p-1]),ei_abs(m_sigma[p-2])),ei_abs(e[p-2])),
00372                         ei_abs(m_sigma[k])),ei_abs(e[k]));
00373         Scalar sp = m_sigma[p-1]/scale;
00374         Scalar spm1 = m_sigma[p-2]/scale;
00375         Scalar epm1 = e[p-2]/scale;
00376         Scalar sk = m_sigma[k]/scale;
00377         Scalar ek = e[k]/scale;
00378         Scalar b = ((spm1 + sp)*(spm1 - sp) + epm1*epm1)/Scalar(2);
00379         Scalar c = (sp*epm1)*(sp*epm1);
00380         Scalar shift(0);
00381         if ((b != 0.0) || (c != 0.0))
00382         {
00383           shift = ei_sqrt(b*b + c);
00384           if (b < 0.0)
00385             shift = -shift;
00386           shift = c/(b + shift);
00387         }
00388         Scalar f = (sk + sp)*(sk - sp) + shift;
00389         Scalar g = sk*ek;
00390 
00391         // Chase zeros.
00392 
00393         for (j = k; j < p-1; ++j)
00394         {
00395           Scalar t = numext::hypot(f,g);
00396           Scalar cs = f/t;
00397           Scalar sn = g/t;
00398           if (j != k)
00399             e[j-1] = t;
00400           f = cs*m_sigma[j] + sn*e[j];
00401           e[j] = cs*e[j] - sn*m_sigma[j];
00402           g = sn*m_sigma[j+1];
00403           m_sigma[j+1] = cs*m_sigma[j+1];
00404           if (wantv)
00405           {
00406             for (i = 0; i < n; ++i)
00407             {
00408               t = cs*m_matV(i,j) + sn*m_matV(i,j+1);
00409               m_matV(i,j+1) = -sn*m_matV(i,j) + cs*m_matV(i,j+1);
00410               m_matV(i,j) = t;
00411             }
00412           }
00413           t = numext::hypot(f,g);
00414           cs = f/t;
00415           sn = g/t;
00416           m_sigma[j] = t;
00417           f = cs*e[j] + sn*m_sigma[j+1];
00418           m_sigma[j+1] = -sn*e[j] + cs*m_sigma[j+1];
00419           g = sn*e[j+1];
00420           e[j+1] = cs*e[j+1];
00421           if (wantu && (j < m-1))
00422           {
00423             for (i = 0; i < m; ++i)
00424             {
00425               t = cs*m_matU(i,j) + sn*m_matU(i,j+1);
00426               m_matU(i,j+1) = -sn*m_matU(i,j) + cs*m_matU(i,j+1);
00427               m_matU(i,j) = t;
00428             }
00429           }
00430         }
00431         e[p-2] = f;
00432         iter = iter + 1;
00433       }
00434       break;
00435 
00436       // Convergence.
00437       case 4:
00438       {
00439         // Make the singular values positive.
00440         if (m_sigma[k] <= 0.0)
00441         {
00442           m_sigma[k] = m_sigma[k] < Scalar(0) ? -m_sigma[k] : Scalar(0);
00443           if (wantv)
00444             m_matV.col(k).start(pp+1) = -m_matV.col(k).start(pp+1);
00445         }
00446 
00447         // Order the singular values.
00448         while (k < pp)
00449         {
00450           if (m_sigma[k] >= m_sigma[k+1])
00451             break;
00452           Scalar t = m_sigma[k];
00453           m_sigma[k] = m_sigma[k+1];
00454           m_sigma[k+1] = t;
00455           if (wantv && (k < n-1))
00456             m_matV.col(k).swap(m_matV.col(k+1));
00457           if (wantu && (k < m-1))
00458             m_matU.col(k).swap(m_matU.col(k+1));
00459           ++k;
00460         }
00461         iter = 0;
00462         p--;
00463       }
00464       break;
00465     } // end big switch
00466   } // end iterations
00467 }
00468 
00469 template<typename MatrixType>
00470 SVD<MatrixType>& SVD<MatrixType>::sort()
00471 {
00472   int mu = m_matU.rows();
00473   int mv = m_matV.rows();
00474   int n  = m_matU.cols();
00475 
00476   for (int i=0; i<n; ++i)
00477   {
00478     int  k = i;
00479     Scalar p = m_sigma.coeff(i);
00480 
00481     for (int j=i+1; j<n; ++j)
00482     {
00483       if (m_sigma.coeff(j) > p)
00484       {
00485         k = j;
00486         p = m_sigma.coeff(j);
00487       }
00488     }
00489     if (k != i)
00490     {
00491       m_sigma.coeffRef(k) = m_sigma.coeff(i);  // i.e.
00492       m_sigma.coeffRef(i) = p;                 // swaps the i-th and the k-th elements
00493 
00494       int j = mu;
00495       for(int s=0; j!=0; ++s, --j)
00496         std::swap(m_matU.coeffRef(s,i), m_matU.coeffRef(s,k));
00497 
00498       j = mv;
00499       for (int s=0; j!=0; ++s, --j)
00500         std::swap(m_matV.coeffRef(s,i), m_matV.coeffRef(s,k));
00501     }
00502   }
00503   return *this;
00504 }
00505 
00511 template<typename MatrixType>
00512 template<typename OtherDerived, typename ResultType>
00513 bool SVD<MatrixType>::solve(const MatrixBase<OtherDerived> &b, ResultType* result) const
00514 {
00515   const int rows = m_matU.rows();
00516   ei_assert(b.rows() == rows);
00517 
00518   Scalar maxVal = m_sigma.cwise().abs().maxCoeff();
00519   for (int j=0; j<b.cols(); ++j)
00520   {
00521     Matrix<Scalar,MatrixUType::RowsAtCompileTime,1> aux = m_matU.transpose() * b.col(j);
00522 
00523     for (int i = 0; i <m_matU.cols(); ++i)
00524     {
00525       Scalar si = m_sigma.coeff(i);
00526       if (ei_isMuchSmallerThan(ei_abs(si),maxVal))
00527         aux.coeffRef(i) = 0;
00528       else
00529         aux.coeffRef(i) /= si;
00530     }
00531 
00532     result->col(j) = m_matV * aux;
00533   }
00534   return true;
00535 }
00536 
00545 template<typename MatrixType>
00546 template<typename UnitaryType, typename PositiveType>
00547 void SVD<MatrixType>::computeUnitaryPositive(UnitaryType *unitary,
00548                                              PositiveType *positive) const
00549 {
00550   ei_assert(m_matU.cols() == m_matV.cols() && "Polar decomposition is only for square matrices");
00551   if(unitary) *unitary = m_matU * m_matV.adjoint();
00552   if(positive) *positive = m_matV * m_sigma.asDiagonal() * m_matV.adjoint();
00553 }
00554 
00563 template<typename MatrixType>
00564 template<typename UnitaryType, typename PositiveType>
00565 void SVD<MatrixType>::computePositiveUnitary(UnitaryType *positive,
00566                                              PositiveType *unitary) const
00567 {
00568   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
00569   if(unitary) *unitary = m_matU * m_matV.adjoint();
00570   if(positive) *positive = m_matU * m_sigma.asDiagonal() * m_matU.adjoint();
00571 }
00572 
00582 template<typename MatrixType>
00583 template<typename RotationType, typename ScalingType>
00584 void SVD<MatrixType>::computeRotationScaling(RotationType *rotation, ScalingType *scaling) const
00585 {
00586   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
00587   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
00588   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
00589   sv.coeffRef(0) *= x;
00590   if(scaling) scaling->lazyAssign(m_matV * sv.asDiagonal() * m_matV.adjoint());
00591   if(rotation)
00592   {
00593     MatrixType m(m_matU);
00594     m.col(0) /= x;
00595     rotation->lazyAssign(m * m_matV.adjoint());
00596   }
00597 }
00598 
00608 template<typename MatrixType>
00609 template<typename ScalingType, typename RotationType>
00610 void SVD<MatrixType>::computeScalingRotation(ScalingType *scaling, RotationType *rotation) const
00611 {
00612   ei_assert(m_matU.rows() == m_matV.rows() && "Polar decomposition is only for square matrices");
00613   Scalar x = (m_matU * m_matV.adjoint()).determinant(); // so x has absolute value 1
00614   Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> sv(m_sigma);
00615   sv.coeffRef(0) *= x;
00616   if(scaling) scaling->lazyAssign(m_matU * sv.asDiagonal() * m_matU.adjoint());
00617   if(rotation)
00618   {
00619     MatrixType m(m_matU);
00620     m.col(0) /= x;
00621     rotation->lazyAssign(m * m_matV.adjoint());
00622   }
00623 }
00624 
00625 
00629 template<typename Derived>
00630 inline SVD<typename MatrixBase<Derived>::PlainObject>
00631 MatrixBase<Derived>::svd() const
00632 {
00633   return SVD<PlainObject>(derived());
00634 }
00635 
00636 } // end namespace Eigen
00637 
00638 #endif // EIGEN2_SVD_H


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:39:55