Camera tracking integration
[blender.git] / extern / Eigen3 / Eigen / src / Eigenvalues / SelfAdjointEigenSolver.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
6 //
7 // Eigen is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU Lesser General Public
9 // License as published by the Free Software Foundation; either
10 // version 3 of the License, or (at your option) any later version.
11 //
12 // Alternatively, you can redistribute it and/or
13 // modify it under the terms of the GNU General Public License as
14 // published by the Free Software Foundation; either version 2 of
15 // the License, or (at your option) any later version.
16 //
17 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
19 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
20 // GNU General Public License for more details.
21 //
22 // You should have received a copy of the GNU Lesser General Public
23 // License and a copy of the GNU General Public License along with
24 // Eigen. If not, see <http://www.gnu.org/licenses/>.
25
26 #ifndef EIGEN_SELFADJOINTEIGENSOLVER_H
27 #define EIGEN_SELFADJOINTEIGENSOLVER_H
28
29 #include "./EigenvaluesCommon.h"
30 #include "./Tridiagonalization.h"
31
32 template<typename _MatrixType>
33 class GeneralizedSelfAdjointEigenSolver;
34
35 /** \eigenvalues_module \ingroup Eigenvalues_Module
36   *
37   *
38   * \class SelfAdjointEigenSolver
39   *
40   * \brief Computes eigenvalues and eigenvectors of selfadjoint matrices
41   *
42   * \tparam _MatrixType the type of the matrix of which we are computing the
43   * eigendecomposition; this is expected to be an instantiation of the Matrix
44   * class template.
45   *
46   * A matrix \f$ A \f$ is selfadjoint if it equals its adjoint. For real
47   * matrices, this means that the matrix is symmetric: it equals its
48   * transpose. This class computes the eigenvalues and eigenvectors of a
49   * selfadjoint matrix. These are the scalars \f$ \lambda \f$ and vectors
50   * \f$ v \f$ such that \f$ Av = \lambda v \f$.  The eigenvalues of a
51   * selfadjoint matrix are always real. If \f$ D \f$ is a diagonal matrix with
52   * the eigenvalues on the diagonal, and \f$ V \f$ is a matrix with the
53   * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$ (for selfadjoint
54   * matrices, the matrix \f$ V \f$ is always invertible). This is called the
55   * eigendecomposition.
56   *
57   * The algorithm exploits the fact that the matrix is selfadjoint, making it
58   * faster and more accurate than the general purpose eigenvalue algorithms
59   * implemented in EigenSolver and ComplexEigenSolver.
60   *
61   * Only the \b lower \b triangular \b part of the input matrix is referenced.
62   *
63   * Call the function compute() to compute the eigenvalues and eigenvectors of
64   * a given matrix. Alternatively, you can use the
65   * SelfAdjointEigenSolver(const MatrixType&, int) constructor which computes
66   * the eigenvalues and eigenvectors at construction time. Once the eigenvalue
67   * and eigenvectors are computed, they can be retrieved with the eigenvalues()
68   * and eigenvectors() functions.
69   *
70   * The documentation for SelfAdjointEigenSolver(const MatrixType&, int)
71   * contains an example of the typical use of this class.
72   *
73   * To solve the \em generalized eigenvalue problem \f$ Av = \lambda Bv \f$ and
74   * the likes, see the class GeneralizedSelfAdjointEigenSolver.
75   *
76   * \sa MatrixBase::eigenvalues(), class EigenSolver, class ComplexEigenSolver
77   */
78 template<typename _MatrixType> class SelfAdjointEigenSolver
79 {
80   public:
81
82     typedef _MatrixType MatrixType;
83     enum {
84       Size = MatrixType::RowsAtCompileTime,
85       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
86       Options = MatrixType::Options,
87       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
88     };
89
90     /** \brief Scalar type for matrices of type \p _MatrixType. */
91     typedef typename MatrixType::Scalar Scalar;
92     typedef typename MatrixType::Index Index;
93
94     /** \brief Real scalar type for \p _MatrixType.
95       *
96       * This is just \c Scalar if #Scalar is real (e.g., \c float or
97       * \c double), and the type of the real part of \c Scalar if #Scalar is
98       * complex.
99       */
100     typedef typename NumTraits<Scalar>::Real RealScalar;
101
102     /** \brief Type for vector of eigenvalues as returned by eigenvalues().
103       *
104       * This is a column vector with entries of type #RealScalar.
105       * The length of the vector is the size of \p _MatrixType.
106       */
107     typedef typename internal::plain_col_type<MatrixType, RealScalar>::type RealVectorType;
108     typedef Tridiagonalization<MatrixType> TridiagonalizationType;
109
110     /** \brief Default constructor for fixed-size matrices.
111       *
112       * The default constructor is useful in cases in which the user intends to
113       * perform decompositions via compute(). This constructor
114       * can only be used if \p _MatrixType is a fixed-size matrix; use
115       * SelfAdjointEigenSolver(Index) for dynamic-size matrices.
116       *
117       * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver.cpp
118       * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver.out
119       */
120     SelfAdjointEigenSolver()
121         : m_eivec(),
122           m_eivalues(),
123           m_subdiag(),
124           m_isInitialized(false)
125     { }
126
127     /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
128       *
129       * \param [in]  size  Positive integer, size of the matrix whose
130       * eigenvalues and eigenvectors will be computed.
131       *
132       * This constructor is useful for dynamic-size matrices, when the user
133       * intends to perform decompositions via compute(). The \p size
134       * parameter is only used as a hint. It is not an error to give a wrong
135       * \p size, but it may impair performance.
136       *
137       * \sa compute() for an example
138       */
139     SelfAdjointEigenSolver(Index size)
140         : m_eivec(size, size),
141           m_eivalues(size),
142           m_subdiag(size > 1 ? size - 1 : 1),
143           m_isInitialized(false)
144     {}
145
146     /** \brief Constructor; computes eigendecomposition of given matrix.
147       *
148       * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
149       *    be computed. Only the lower triangular part of the matrix is referenced.
150       * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
151       *
152       * This constructor calls compute(const MatrixType&, int) to compute the
153       * eigenvalues of the matrix \p matrix. The eigenvectors are computed if
154       * \p options equals #ComputeEigenvectors.
155       *
156       * Example: \include SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.cpp
157       * Output: \verbinclude SelfAdjointEigenSolver_SelfAdjointEigenSolver_MatrixType.out
158       *
159       * \sa compute(const MatrixType&, int)
160       */
161     SelfAdjointEigenSolver(const MatrixType& matrix, int options = ComputeEigenvectors)
162       : m_eivec(matrix.rows(), matrix.cols()),
163         m_eivalues(matrix.cols()),
164         m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
165         m_isInitialized(false)
166     {
167       compute(matrix, options);
168     }
169
170     /** \brief Computes eigendecomposition of given matrix.
171       *
172       * \param[in]  matrix  Selfadjoint matrix whose eigendecomposition is to
173       *    be computed. Only the lower triangular part of the matrix is referenced.
174       * \param[in]  options Can be #ComputeEigenvectors (default) or #EigenvaluesOnly.
175       * \returns    Reference to \c *this
176       *
177       * This function computes the eigenvalues of \p matrix.  The eigenvalues()
178       * function can be used to retrieve them.  If \p options equals #ComputeEigenvectors,
179       * then the eigenvectors are also computed and can be retrieved by
180       * calling eigenvectors().
181       *
182       * This implementation uses a symmetric QR algorithm. The matrix is first
183       * reduced to tridiagonal form using the Tridiagonalization class. The
184       * tridiagonal matrix is then brought to diagonal form with implicit
185       * symmetric QR steps with Wilkinson shift. Details can be found in
186       * Section 8.3 of Golub \& Van Loan, <i>%Matrix Computations</i>.
187       *
188       * The cost of the computation is about \f$ 9n^3 \f$ if the eigenvectors
189       * are required and \f$ 4n^3/3 \f$ if they are not required.
190       *
191       * This method reuses the memory in the SelfAdjointEigenSolver object that
192       * was allocated when the object was constructed, if the size of the
193       * matrix does not change.
194       *
195       * Example: \include SelfAdjointEigenSolver_compute_MatrixType.cpp
196       * Output: \verbinclude SelfAdjointEigenSolver_compute_MatrixType.out
197       *
198       * \sa SelfAdjointEigenSolver(const MatrixType&, int)
199       */
200     SelfAdjointEigenSolver& compute(const MatrixType& matrix, int options = ComputeEigenvectors);
201
202     /** \brief Returns the eigenvectors of given matrix.
203       *
204       * \returns  A const reference to the matrix whose columns are the eigenvectors.
205       *
206       * \pre The eigenvectors have been computed before.
207       *
208       * Column \f$ k \f$ of the returned matrix is an eigenvector corresponding
209       * to eigenvalue number \f$ k \f$ as returned by eigenvalues().  The
210       * eigenvectors are normalized to have (Euclidean) norm equal to one. If
211       * this object was used to solve the eigenproblem for the selfadjoint
212       * matrix \f$ A \f$, then the matrix returned by this function is the
213       * matrix \f$ V \f$ in the eigendecomposition \f$ A = V D V^{-1} \f$.
214       *
215       * Example: \include SelfAdjointEigenSolver_eigenvectors.cpp
216       * Output: \verbinclude SelfAdjointEigenSolver_eigenvectors.out
217       *
218       * \sa eigenvalues()
219       */
220     const MatrixType& eigenvectors() const
221     {
222       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
223       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
224       return m_eivec;
225     }
226
227     /** \brief Returns the eigenvalues of given matrix.
228       *
229       * \returns A const reference to the column vector containing the eigenvalues.
230       *
231       * \pre The eigenvalues have been computed before.
232       *
233       * The eigenvalues are repeated according to their algebraic multiplicity,
234       * so there are as many eigenvalues as rows in the matrix. The eigenvalues
235       * are sorted in increasing order.
236       *
237       * Example: \include SelfAdjointEigenSolver_eigenvalues.cpp
238       * Output: \verbinclude SelfAdjointEigenSolver_eigenvalues.out
239       *
240       * \sa eigenvectors(), MatrixBase::eigenvalues()
241       */
242     const RealVectorType& eigenvalues() const
243     {
244       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
245       return m_eivalues;
246     }
247
248     /** \brief Computes the positive-definite square root of the matrix.
249       *
250       * \returns the positive-definite square root of the matrix
251       *
252       * \pre The eigenvalues and eigenvectors of a positive-definite matrix
253       * have been computed before.
254       *
255       * The square root of a positive-definite matrix \f$ A \f$ is the
256       * positive-definite matrix whose square equals \f$ A \f$. This function
257       * uses the eigendecomposition \f$ A = V D V^{-1} \f$ to compute the
258       * square root as \f$ A^{1/2} = V D^{1/2} V^{-1} \f$.
259       *
260       * Example: \include SelfAdjointEigenSolver_operatorSqrt.cpp
261       * Output: \verbinclude SelfAdjointEigenSolver_operatorSqrt.out
262       *
263       * \sa operatorInverseSqrt(),
264       *     \ref MatrixFunctions_Module "MatrixFunctions Module"
265       */
266     MatrixType operatorSqrt() const
267     {
268       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
269       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
270       return m_eivec * m_eivalues.cwiseSqrt().asDiagonal() * m_eivec.adjoint();
271     }
272
273     /** \brief Computes the inverse square root of the matrix.
274       *
275       * \returns the inverse positive-definite square root of the matrix
276       *
277       * \pre The eigenvalues and eigenvectors of a positive-definite matrix
278       * have been computed before.
279       *
280       * This function uses the eigendecomposition \f$ A = V D V^{-1} \f$ to
281       * compute the inverse square root as \f$ V D^{-1/2} V^{-1} \f$. This is
282       * cheaper than first computing the square root with operatorSqrt() and
283       * then its inverse with MatrixBase::inverse().
284       *
285       * Example: \include SelfAdjointEigenSolver_operatorInverseSqrt.cpp
286       * Output: \verbinclude SelfAdjointEigenSolver_operatorInverseSqrt.out
287       *
288       * \sa operatorSqrt(), MatrixBase::inverse(),
289       *     \ref MatrixFunctions_Module "MatrixFunctions Module"
290       */
291     MatrixType operatorInverseSqrt() const
292     {
293       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
294       eigen_assert(m_eigenvectorsOk && "The eigenvectors have not been computed together with the eigenvalues.");
295       return m_eivec * m_eivalues.cwiseInverse().cwiseSqrt().asDiagonal() * m_eivec.adjoint();
296     }
297
298     /** \brief Reports whether previous computation was successful.
299       *
300       * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
301       */
302     ComputationInfo info() const
303     {
304       eigen_assert(m_isInitialized && "SelfAdjointEigenSolver is not initialized.");
305       return m_info;
306     }
307
308     /** \brief Maximum number of iterations.
309       *
310       * Maximum number of iterations allowed for an eigenvalue to converge.
311       */
312     static const int m_maxIterations = 30;
313
314     #ifdef EIGEN2_SUPPORT
315     SelfAdjointEigenSolver(const MatrixType& matrix, bool computeEigenvectors)
316       : m_eivec(matrix.rows(), matrix.cols()),
317         m_eivalues(matrix.cols()),
318         m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
319         m_isInitialized(false)
320     {
321       compute(matrix, computeEigenvectors);
322     }
323     
324     SelfAdjointEigenSolver(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
325         : m_eivec(matA.cols(), matA.cols()),
326           m_eivalues(matA.cols()),
327           m_subdiag(matA.cols() > 1 ? matA.cols() - 1 : 1),
328           m_isInitialized(false)
329     {
330       static_cast<GeneralizedSelfAdjointEigenSolver<MatrixType>*>(this)->compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
331     }
332     
333     void compute(const MatrixType& matrix, bool computeEigenvectors)
334     {
335       compute(matrix, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
336     }
337
338     void compute(const MatrixType& matA, const MatrixType& matB, bool computeEigenvectors = true)
339     {
340       compute(matA, matB, computeEigenvectors ? ComputeEigenvectors : EigenvaluesOnly);
341     }
342     #endif // EIGEN2_SUPPORT
343
344   protected:
345     MatrixType m_eivec;
346     RealVectorType m_eivalues;
347     typename TridiagonalizationType::SubDiagonalType m_subdiag;
348     ComputationInfo m_info;
349     bool m_isInitialized;
350     bool m_eigenvectorsOk;
351 };
352
353 /** \internal
354   *
355   * \eigenvalues_module \ingroup Eigenvalues_Module
356   *
357   * Performs a QR step on a tridiagonal symmetric matrix represented as a
358   * pair of two vectors \a diag and \a subdiag.
359   *
360   * \param matA the input selfadjoint matrix
361   * \param hCoeffs returned Householder coefficients
362   *
363   * For compilation efficiency reasons, this procedure does not use eigen expression
364   * for its arguments.
365   *
366   * Implemented from Golub's "Matrix Computations", algorithm 8.3.2:
367   * "implicit symmetric QR step with Wilkinson shift"
368   */
369 namespace internal {
370 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
371 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n);
372 }
373
374 template<typename MatrixType>
375 SelfAdjointEigenSolver<MatrixType>& SelfAdjointEigenSolver<MatrixType>
376 ::compute(const MatrixType& matrix, int options)
377 {
378   eigen_assert(matrix.cols() == matrix.rows());
379   eigen_assert((options&~(EigVecMask|GenEigMask))==0
380           && (options&EigVecMask)!=EigVecMask
381           && "invalid option parameter");
382   bool computeEigenvectors = (options&ComputeEigenvectors)==ComputeEigenvectors;
383   Index n = matrix.cols();
384   m_eivalues.resize(n,1);
385
386   if(n==1)
387   {
388     m_eivalues.coeffRef(0,0) = internal::real(matrix.coeff(0,0));
389     if(computeEigenvectors)
390       m_eivec.setOnes(n,n);
391     m_info = Success;
392     m_isInitialized = true;
393     m_eigenvectorsOk = computeEigenvectors;
394     return *this;
395   }
396
397   // declare some aliases
398   RealVectorType& diag = m_eivalues;
399   MatrixType& mat = m_eivec;
400
401   // map the matrix coefficients to [-1:1] to avoid over- and underflow.
402   RealScalar scale = matrix.cwiseAbs().maxCoeff();
403   if(scale==Scalar(0)) scale = 1;
404   mat = matrix / scale;
405   m_subdiag.resize(n-1);
406   internal::tridiagonalization_inplace(mat, diag, m_subdiag, computeEigenvectors);
407   
408   Index end = n-1;
409   Index start = 0;
410   Index iter = 0; // number of iterations we are working on one element
411
412   while (end>0)
413   {
414     for (Index i = start; i<end; ++i)
415       if (internal::isMuchSmallerThan(internal::abs(m_subdiag[i]),(internal::abs(diag[i])+internal::abs(diag[i+1]))))
416         m_subdiag[i] = 0;
417
418     // find the largest unreduced block
419     while (end>0 && m_subdiag[end-1]==0)
420     {
421       iter = 0;
422       end--;
423     }
424     if (end<=0)
425       break;
426
427     // if we spent too many iterations on the current element, we give up
428     iter++;
429     if(iter > m_maxIterations) break;
430
431     start = end - 1;
432     while (start>0 && m_subdiag[start-1]!=0)
433       start--;
434
435     internal::tridiagonal_qr_step<MatrixType::Flags&RowMajorBit ? RowMajor : ColMajor>(diag.data(), m_subdiag.data(), start, end, computeEigenvectors ? m_eivec.data() : (Scalar*)0, n);
436   }
437
438   if (iter <= m_maxIterations)
439     m_info = Success;
440   else
441     m_info = NoConvergence;
442
443   // Sort eigenvalues and corresponding vectors.
444   // TODO make the sort optional ?
445   // TODO use a better sort algorithm !!
446   if (m_info == Success)
447   {
448     for (Index i = 0; i < n-1; ++i)
449     {
450       Index k;
451       m_eivalues.segment(i,n-i).minCoeff(&k);
452       if (k > 0)
453       {
454         std::swap(m_eivalues[i], m_eivalues[k+i]);
455         if(computeEigenvectors)
456           m_eivec.col(i).swap(m_eivec.col(k+i));
457       }
458     }
459   }
460   
461   // scale back the eigen values
462   m_eivalues *= scale;
463
464   m_isInitialized = true;
465   m_eigenvectorsOk = computeEigenvectors;
466   return *this;
467 }
468
469 namespace internal {
470 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
471 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
472 {
473   // NOTE this version avoids over & underflow, however since the matrix is prescaled, overflow cannot occur,
474   // and underflows should be meaningless anyway. So I don't any reason to enable this version, but I keep
475   // it here for reference:
476 //   RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
477 //   RealScalar e = subdiag[end-1];
478 //   RealScalar mu = diag[end] - (e / (td + (td>0 ? 1 : -1))) * (e / hypot(td,e));
479   RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
480   RealScalar e2 = abs2(subdiag[end-1]);
481   RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
482   RealScalar x = diag[start] - mu;
483   RealScalar z = subdiag[start];
484   for (Index k = start; k < end; ++k)
485   {
486     JacobiRotation<RealScalar> rot;
487     rot.makeGivens(x, z);
488
489     // do T = G' T G
490     RealScalar sdk = rot.s() * diag[k] + rot.c() * subdiag[k];
491     RealScalar dkp1 = rot.s() * subdiag[k] + rot.c() * diag[k+1];
492
493     diag[k] = rot.c() * (rot.c() * diag[k] - rot.s() * subdiag[k]) - rot.s() * (rot.c() * subdiag[k] - rot.s() * diag[k+1]);
494     diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
495     subdiag[k] = rot.c() * sdk - rot.s() * dkp1;
496     
497
498     if (k > start)
499       subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;
500
501     x = subdiag[k];
502
503     if (k < end - 1)
504     {
505       z = -rot.s() * subdiag[k+1];
506       subdiag[k + 1] = rot.c() * subdiag[k+1];
507     }
508     
509     // apply the givens rotation to the unit matrix Q = Q * G
510     if (matrixQ)
511     {
512       // FIXME if StorageOrder == RowMajor this operation is not very efficient
513       Map<Matrix<Scalar,Dynamic,Dynamic,StorageOrder> > q(matrixQ,n,n);
514       q.applyOnTheRight(k,k+1,rot);
515     }
516   }
517 }
518 } // end namespace internal
519
520 #endif // EIGEN_SELFADJOINTEIGENSOLVER_H