Camera tracking integration
[blender.git] / extern / Eigen3 / Eigen / src / QR / ColPivHouseholderQR.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
5 // Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
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_COLPIVOTINGHOUSEHOLDERQR_H
27 #define EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
28
29 /** \ingroup QR_Module
30   *
31   * \class ColPivHouseholderQR
32   *
33   * \brief Householder rank-revealing QR decomposition of a matrix with column-pivoting
34   *
35   * \param MatrixType the type of the matrix of which we are computing the QR decomposition
36   *
37   * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b Q and \b R
38   * such that 
39   * \f[
40   *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \, \mathbf{R}
41   * \f]
42   * by using Householder transformations. Here, \b P is a permutation matrix, \b Q a unitary matrix and \b R an 
43   * upper triangular matrix.
44   *
45   * This decomposition performs column pivoting in order to be rank-revealing and improve
46   * numerical stability. It is slower than HouseholderQR, and faster than FullPivHouseholderQR.
47   *
48   * \sa MatrixBase::colPivHouseholderQr()
49   */
50 template<typename _MatrixType> class ColPivHouseholderQR
51 {
52   public:
53
54     typedef _MatrixType MatrixType;
55     enum {
56       RowsAtCompileTime = MatrixType::RowsAtCompileTime,
57       ColsAtCompileTime = MatrixType::ColsAtCompileTime,
58       Options = MatrixType::Options,
59       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
60       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
61     };
62     typedef typename MatrixType::Scalar Scalar;
63     typedef typename MatrixType::RealScalar RealScalar;
64     typedef typename MatrixType::Index Index;
65     typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, Options, MaxRowsAtCompileTime, MaxRowsAtCompileTime> MatrixQType;
66     typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
67     typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
68     typedef typename internal::plain_row_type<MatrixType, Index>::type IntRowVectorType;
69     typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
70     typedef typename internal::plain_row_type<MatrixType, RealScalar>::type RealRowVectorType;
71     typedef typename HouseholderSequence<MatrixType,HCoeffsType>::ConjugateReturnType HouseholderSequenceType;
72
73     /**
74     * \brief Default Constructor.
75     *
76     * The default constructor is useful in cases in which the user intends to
77     * perform decompositions via ColPivHouseholderQR::compute(const MatrixType&).
78     */
79     ColPivHouseholderQR()
80       : m_qr(),
81         m_hCoeffs(),
82         m_colsPermutation(),
83         m_colsTranspositions(),
84         m_temp(),
85         m_colSqNorms(),
86         m_isInitialized(false) {}
87
88     /** \brief Default Constructor with memory preallocation
89       *
90       * Like the default constructor but with preallocation of the internal data
91       * according to the specified problem \a size.
92       * \sa ColPivHouseholderQR()
93       */
94     ColPivHouseholderQR(Index rows, Index cols)
95       : m_qr(rows, cols),
96         m_hCoeffs((std::min)(rows,cols)),
97         m_colsPermutation(cols),
98         m_colsTranspositions(cols),
99         m_temp(cols),
100         m_colSqNorms(cols),
101         m_isInitialized(false),
102         m_usePrescribedThreshold(false) {}
103
104     ColPivHouseholderQR(const MatrixType& matrix)
105       : m_qr(matrix.rows(), matrix.cols()),
106         m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
107         m_colsPermutation(matrix.cols()),
108         m_colsTranspositions(matrix.cols()),
109         m_temp(matrix.cols()),
110         m_colSqNorms(matrix.cols()),
111         m_isInitialized(false),
112         m_usePrescribedThreshold(false)
113     {
114       compute(matrix);
115     }
116
117     /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
118       * *this is the QR decomposition, if any exists.
119       *
120       * \param b the right-hand-side of the equation to solve.
121       *
122       * \returns a solution.
123       *
124       * \note The case where b is a matrix is not yet implemented. Also, this
125       *       code is space inefficient.
126       *
127       * \note_about_checking_solutions
128       *
129       * \note_about_arbitrary_choice_of_solution
130       *
131       * Example: \include ColPivHouseholderQR_solve.cpp
132       * Output: \verbinclude ColPivHouseholderQR_solve.out
133       */
134     template<typename Rhs>
135     inline const internal::solve_retval<ColPivHouseholderQR, Rhs>
136     solve(const MatrixBase<Rhs>& b) const
137     {
138       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
139       return internal::solve_retval<ColPivHouseholderQR, Rhs>(*this, b.derived());
140     }
141
142     HouseholderSequenceType householderQ(void) const;
143
144     /** \returns a reference to the matrix where the Householder QR decomposition is stored
145       */
146     const MatrixType& matrixQR() const
147     {
148       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
149       return m_qr;
150     }
151
152     ColPivHouseholderQR& compute(const MatrixType& matrix);
153
154     const PermutationType& colsPermutation() const
155     {
156       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
157       return m_colsPermutation;
158     }
159
160     /** \returns the absolute value of the determinant of the matrix of which
161       * *this is the QR decomposition. It has only linear complexity
162       * (that is, O(n) where n is the dimension of the square matrix)
163       * as the QR decomposition has already been computed.
164       *
165       * \note This is only for square matrices.
166       *
167       * \warning a determinant can be very big or small, so for matrices
168       * of large enough dimension, there is a risk of overflow/underflow.
169       * One way to work around that is to use logAbsDeterminant() instead.
170       *
171       * \sa logAbsDeterminant(), MatrixBase::determinant()
172       */
173     typename MatrixType::RealScalar absDeterminant() const;
174
175     /** \returns the natural log of the absolute value of the determinant of the matrix of which
176       * *this is the QR decomposition. It has only linear complexity
177       * (that is, O(n) where n is the dimension of the square matrix)
178       * as the QR decomposition has already been computed.
179       *
180       * \note This is only for square matrices.
181       *
182       * \note This method is useful to work around the risk of overflow/underflow that's inherent
183       * to determinant computation.
184       *
185       * \sa absDeterminant(), MatrixBase::determinant()
186       */
187     typename MatrixType::RealScalar logAbsDeterminant() const;
188
189     /** \returns the rank of the matrix of which *this is the QR decomposition.
190       *
191       * \note This method has to determine which pivots should be considered nonzero.
192       *       For that, it uses the threshold value that you can control by calling
193       *       setThreshold(const RealScalar&).
194       */
195     inline Index rank() const
196     {
197       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
198       RealScalar premultiplied_threshold = internal::abs(m_maxpivot) * threshold();
199       Index result = 0;
200       for(Index i = 0; i < m_nonzero_pivots; ++i)
201         result += (internal::abs(m_qr.coeff(i,i)) > premultiplied_threshold);
202       return result;
203     }
204
205     /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
206       *
207       * \note This method has to determine which pivots should be considered nonzero.
208       *       For that, it uses the threshold value that you can control by calling
209       *       setThreshold(const RealScalar&).
210       */
211     inline Index dimensionOfKernel() const
212     {
213       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
214       return cols() - rank();
215     }
216
217     /** \returns true if the matrix of which *this is the QR decomposition represents an injective
218       *          linear map, i.e. has trivial kernel; false otherwise.
219       *
220       * \note This method has to determine which pivots should be considered nonzero.
221       *       For that, it uses the threshold value that you can control by calling
222       *       setThreshold(const RealScalar&).
223       */
224     inline bool isInjective() const
225     {
226       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
227       return rank() == cols();
228     }
229
230     /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
231       *          linear map; false otherwise.
232       *
233       * \note This method has to determine which pivots should be considered nonzero.
234       *       For that, it uses the threshold value that you can control by calling
235       *       setThreshold(const RealScalar&).
236       */
237     inline bool isSurjective() const
238     {
239       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
240       return rank() == rows();
241     }
242
243     /** \returns true if the matrix of which *this is the QR decomposition is invertible.
244       *
245       * \note This method has to determine which pivots should be considered nonzero.
246       *       For that, it uses the threshold value that you can control by calling
247       *       setThreshold(const RealScalar&).
248       */
249     inline bool isInvertible() const
250     {
251       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
252       return isInjective() && isSurjective();
253     }
254
255     /** \returns the inverse of the matrix of which *this is the QR decomposition.
256       *
257       * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
258       *       Use isInvertible() to first determine whether this matrix is invertible.
259       */
260     inline const
261     internal::solve_retval<ColPivHouseholderQR, typename MatrixType::IdentityReturnType>
262     inverse() const
263     {
264       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
265       return internal::solve_retval<ColPivHouseholderQR,typename MatrixType::IdentityReturnType>
266                (*this, MatrixType::Identity(m_qr.rows(), m_qr.cols()));
267     }
268
269     inline Index rows() const { return m_qr.rows(); }
270     inline Index cols() const { return m_qr.cols(); }
271     const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
272
273     /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
274       * who need to determine when pivots are to be considered nonzero. This is not used for the
275       * QR decomposition itself.
276       *
277       * When it needs to get the threshold value, Eigen calls threshold(). By default, this
278       * uses a formula to automatically determine a reasonable threshold.
279       * Once you have called the present method setThreshold(const RealScalar&),
280       * your value is used instead.
281       *
282       * \param threshold The new value to use as the threshold.
283       *
284       * A pivot will be considered nonzero if its absolute value is strictly greater than
285       *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
286       * where maxpivot is the biggest pivot.
287       *
288       * If you want to come back to the default behavior, call setThreshold(Default_t)
289       */
290     ColPivHouseholderQR& setThreshold(const RealScalar& threshold)
291     {
292       m_usePrescribedThreshold = true;
293       m_prescribedThreshold = threshold;
294       return *this;
295     }
296
297     /** Allows to come back to the default behavior, letting Eigen use its default formula for
298       * determining the threshold.
299       *
300       * You should pass the special object Eigen::Default as parameter here.
301       * \code qr.setThreshold(Eigen::Default); \endcode
302       *
303       * See the documentation of setThreshold(const RealScalar&).
304       */
305     ColPivHouseholderQR& setThreshold(Default_t)
306     {
307       m_usePrescribedThreshold = false;
308       return *this;
309     }
310
311     /** Returns the threshold that will be used by certain methods such as rank().
312       *
313       * See the documentation of setThreshold(const RealScalar&).
314       */
315     RealScalar threshold() const
316     {
317       eigen_assert(m_isInitialized || m_usePrescribedThreshold);
318       return m_usePrescribedThreshold ? m_prescribedThreshold
319       // this formula comes from experimenting (see "LU precision tuning" thread on the list)
320       // and turns out to be identical to Higham's formula used already in LDLt.
321                                       : NumTraits<Scalar>::epsilon() * m_qr.diagonalSize();
322     }
323
324     /** \returns the number of nonzero pivots in the QR decomposition.
325       * Here nonzero is meant in the exact sense, not in a fuzzy sense.
326       * So that notion isn't really intrinsically interesting, but it is
327       * still useful when implementing algorithms.
328       *
329       * \sa rank()
330       */
331     inline Index nonzeroPivots() const
332     {
333       eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
334       return m_nonzero_pivots;
335     }
336
337     /** \returns the absolute value of the biggest pivot, i.e. the biggest
338       *          diagonal coefficient of R.
339       */
340     RealScalar maxPivot() const { return m_maxpivot; }
341
342   protected:
343     MatrixType m_qr;
344     HCoeffsType m_hCoeffs;
345     PermutationType m_colsPermutation;
346     IntRowVectorType m_colsTranspositions;
347     RowVectorType m_temp;
348     RealRowVectorType m_colSqNorms;
349     bool m_isInitialized, m_usePrescribedThreshold;
350     RealScalar m_prescribedThreshold, m_maxpivot;
351     Index m_nonzero_pivots;
352     Index m_det_pq;
353 };
354
355 template<typename MatrixType>
356 typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::absDeterminant() const
357 {
358   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
359   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
360   return internal::abs(m_qr.diagonal().prod());
361 }
362
363 template<typename MatrixType>
364 typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::logAbsDeterminant() const
365 {
366   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
367   eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
368   return m_qr.diagonal().cwiseAbs().array().log().sum();
369 }
370
371 template<typename MatrixType>
372 ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const MatrixType& matrix)
373 {
374   Index rows = matrix.rows();
375   Index cols = matrix.cols();
376   Index size = matrix.diagonalSize();
377
378   m_qr = matrix;
379   m_hCoeffs.resize(size);
380
381   m_temp.resize(cols);
382
383   m_colsTranspositions.resize(matrix.cols());
384   Index number_of_transpositions = 0;
385
386   m_colSqNorms.resize(cols);
387   for(Index k = 0; k < cols; ++k)
388     m_colSqNorms.coeffRef(k) = m_qr.col(k).squaredNorm();
389
390   RealScalar threshold_helper = m_colSqNorms.maxCoeff() * internal::abs2(NumTraits<Scalar>::epsilon()) / RealScalar(rows);
391
392   m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
393   m_maxpivot = RealScalar(0);
394
395   for(Index k = 0; k < size; ++k)
396   {
397     // first, we look up in our table m_colSqNorms which column has the biggest squared norm
398     Index biggest_col_index;
399     RealScalar biggest_col_sq_norm = m_colSqNorms.tail(cols-k).maxCoeff(&biggest_col_index);
400     biggest_col_index += k;
401
402     // since our table m_colSqNorms accumulates imprecision at every step, we must now recompute
403     // the actual squared norm of the selected column.
404     // Note that not doing so does result in solve() sometimes returning inf/nan values
405     // when running the unit test with 1000 repetitions.
406     biggest_col_sq_norm = m_qr.col(biggest_col_index).tail(rows-k).squaredNorm();
407
408     // we store that back into our table: it can't hurt to correct our table.
409     m_colSqNorms.coeffRef(biggest_col_index) = biggest_col_sq_norm;
410
411     // if the current biggest column is smaller than epsilon times the initial biggest column,
412     // terminate to avoid generating nan/inf values.
413     // Note that here, if we test instead for "biggest == 0", we get a failure every 1000 (or so)
414     // repetitions of the unit test, with the result of solve() filled with large values of the order
415     // of 1/(size*epsilon).
416     if(biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))
417     {
418       m_nonzero_pivots = k;
419       m_hCoeffs.tail(size-k).setZero();
420       m_qr.bottomRightCorner(rows-k,cols-k)
421           .template triangularView<StrictlyLower>()
422           .setZero();
423       break;
424     }
425
426     // apply the transposition to the columns
427     m_colsTranspositions.coeffRef(k) = biggest_col_index;
428     if(k != biggest_col_index) {
429       m_qr.col(k).swap(m_qr.col(biggest_col_index));
430       std::swap(m_colSqNorms.coeffRef(k), m_colSqNorms.coeffRef(biggest_col_index));
431       ++number_of_transpositions;
432     }
433
434     // generate the householder vector, store it below the diagonal
435     RealScalar beta;
436     m_qr.col(k).tail(rows-k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);
437
438     // apply the householder transformation to the diagonal coefficient
439     m_qr.coeffRef(k,k) = beta;
440
441     // remember the maximum absolute value of diagonal coefficients
442     if(internal::abs(beta) > m_maxpivot) m_maxpivot = internal::abs(beta);
443
444     // apply the householder transformation
445     m_qr.bottomRightCorner(rows-k, cols-k-1)
446         .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));
447
448     // update our table of squared norms of the columns
449     m_colSqNorms.tail(cols-k-1) -= m_qr.row(k).tail(cols-k-1).cwiseAbs2();
450   }
451
452   m_colsPermutation.setIdentity(cols);
453   for(Index k = 0; k < m_nonzero_pivots; ++k)
454     m_colsPermutation.applyTranspositionOnTheRight(k, m_colsTranspositions.coeff(k));
455
456   m_det_pq = (number_of_transpositions%2) ? -1 : 1;
457   m_isInitialized = true;
458
459   return *this;
460 }
461
462 namespace internal {
463
464 template<typename _MatrixType, typename Rhs>
465 struct solve_retval<ColPivHouseholderQR<_MatrixType>, Rhs>
466   : solve_retval_base<ColPivHouseholderQR<_MatrixType>, Rhs>
467 {
468   EIGEN_MAKE_SOLVE_HELPERS(ColPivHouseholderQR<_MatrixType>,Rhs)
469
470   template<typename Dest> void evalTo(Dest& dst) const
471   {
472     eigen_assert(rhs().rows() == dec().rows());
473
474     const int cols = dec().cols(),
475     nonzero_pivots = dec().nonzeroPivots();
476
477     if(nonzero_pivots == 0)
478     {
479       dst.setZero();
480       return;
481     }
482
483     typename Rhs::PlainObject c(rhs());
484
485     // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T
486     c.applyOnTheLeft(householderSequence(dec().matrixQR(), dec().hCoeffs())
487                      .setLength(dec().nonzeroPivots())
488                      .transpose()
489       );
490
491     dec().matrixQR()
492        .topLeftCorner(nonzero_pivots, nonzero_pivots)
493        .template triangularView<Upper>()
494        .solveInPlace(c.topRows(nonzero_pivots));
495
496
497     typename Rhs::PlainObject d(c);
498     d.topRows(nonzero_pivots)
499       = dec().matrixQR()
500        .topLeftCorner(nonzero_pivots, nonzero_pivots)
501        .template triangularView<Upper>()
502        * c.topRows(nonzero_pivots);
503
504     for(Index i = 0; i < nonzero_pivots; ++i) dst.row(dec().colsPermutation().indices().coeff(i)) = c.row(i);
505     for(Index i = nonzero_pivots; i < cols; ++i) dst.row(dec().colsPermutation().indices().coeff(i)).setZero();
506   }
507 };
508
509 } // end namespace internal
510
511 /** \returns the matrix Q as a sequence of householder transformations */
512 template<typename MatrixType>
513 typename ColPivHouseholderQR<MatrixType>::HouseholderSequenceType ColPivHouseholderQR<MatrixType>
514   ::householderQ() const
515 {
516   eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
517   return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate()).setLength(m_nonzero_pivots);
518 }
519
520 /** \return the column-pivoting Householder QR decomposition of \c *this.
521   *
522   * \sa class ColPivHouseholderQR
523   */
524 template<typename Derived>
525 const ColPivHouseholderQR<typename MatrixBase<Derived>::PlainObject>
526 MatrixBase<Derived>::colPivHouseholderQr() const
527 {
528   return ColPivHouseholderQR<PlainObject>(eval());
529 }
530
531
532 #endif // EIGEN_COLPIVOTINGHOUSEHOLDERQR_H