ColPivHouseholderQR.h 24.3 KB
Newer Older
LM's avatar
LM committed
1 2 3 4 5 6
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
Don Gagne's avatar
Don Gagne committed
7 8 9
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
LM's avatar
LM committed
10 11 12 13

#ifndef EIGEN_COLPIVOTINGHOUSEHOLDERQR_H
#define EIGEN_COLPIVOTINGHOUSEHOLDERQR_H

14 15 16 17 18 19 20 21 22 23
namespace Eigen {

namespace internal {
template<typename _MatrixType> struct traits<ColPivHouseholderQR<_MatrixType> >
 : traits<_MatrixType>
{
  enum { Flags = 0 };
};

} // end namespace internal
Don Gagne's avatar
Don Gagne committed
24

LM's avatar
LM committed
25 26 27 28 29 30
/** \ingroup QR_Module
  *
  * \class ColPivHouseholderQR
  *
  * \brief Householder rank-revealing QR decomposition of a matrix with column-pivoting
  *
31
  * \tparam _MatrixType the type of the matrix of which we are computing the QR decomposition
LM's avatar
LM committed
32 33
  *
  * This class performs a rank-revealing QR decomposition of a matrix \b A into matrices \b P, \b Q and \b R
34
  * such that
LM's avatar
LM committed
35 36 37
  * \f[
  *  \mathbf{A} \, \mathbf{P} = \mathbf{Q} \, \mathbf{R}
  * \f]
38
  * by using Householder transformations. Here, \b P is a permutation matrix, \b Q a unitary matrix and \b R an
LM's avatar
LM committed
39 40 41 42 43
  * upper triangular matrix.
  *
  * This decomposition performs column pivoting in order to be rank-revealing and improve
  * numerical stability. It is slower than HouseholderQR, and faster than FullPivHouseholderQR.
  *
44 45
  * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
  * 
LM's avatar
LM committed
46 47 48 49 50 51 52 53 54 55 56 57 58 59 60
  * \sa MatrixBase::colPivHouseholderQr()
  */
template<typename _MatrixType> class ColPivHouseholderQR
{
  public:

    typedef _MatrixType MatrixType;
    enum {
      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
      MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
    };
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::RealScalar RealScalar;
61 62
    // FIXME should be int
    typedef typename MatrixType::StorageIndex StorageIndex;
LM's avatar
LM committed
63 64 65 66 67
    typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
    typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
    typedef typename internal::plain_row_type<MatrixType, Index>::type IntRowVectorType;
    typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
    typedef typename internal::plain_row_type<MatrixType, RealScalar>::type RealRowVectorType;
Don Gagne's avatar
Don Gagne committed
68
    typedef HouseholderSequence<MatrixType,typename internal::remove_all<typename HCoeffsType::ConjugateReturnType>::type> HouseholderSequenceType;
69 70
    typedef typename MatrixType::PlainObject PlainObject;

Don Gagne's avatar
Don Gagne committed
71
  private:
72 73 74

    typedef typename PermutationType::StorageIndex PermIndexType;

Don Gagne's avatar
Don Gagne committed
75
  public:
LM's avatar
LM committed
76 77 78 79 80 81 82 83 84 85 86 87 88

    /**
    * \brief Default Constructor.
    *
    * The default constructor is useful in cases in which the user intends to
    * perform decompositions via ColPivHouseholderQR::compute(const MatrixType&).
    */
    ColPivHouseholderQR()
      : m_qr(),
        m_hCoeffs(),
        m_colsPermutation(),
        m_colsTranspositions(),
        m_temp(),
89 90
        m_colNormsUpdated(),
        m_colNormsDirect(),
91 92
        m_isInitialized(false),
        m_usePrescribedThreshold(false) {}
LM's avatar
LM committed
93 94 95 96 97 98 99 100 101

    /** \brief Default Constructor with memory preallocation
      *
      * Like the default constructor but with preallocation of the internal data
      * according to the specified problem \a size.
      * \sa ColPivHouseholderQR()
      */
    ColPivHouseholderQR(Index rows, Index cols)
      : m_qr(rows, cols),
Don Gagne's avatar
Don Gagne committed
102 103
        m_hCoeffs((std::min)(rows,cols)),
        m_colsPermutation(PermIndexType(cols)),
LM's avatar
LM committed
104 105
        m_colsTranspositions(cols),
        m_temp(cols),
106 107
        m_colNormsUpdated(cols),
        m_colNormsDirect(cols),
LM's avatar
LM committed
108 109 110
        m_isInitialized(false),
        m_usePrescribedThreshold(false) {}

Don Gagne's avatar
Don Gagne committed
111 112 113 114
    /** \brief Constructs a QR factorization from a given matrix
      *
      * This constructor computes the QR factorization of the matrix \a matrix by calling
      * the method compute(). It is a short cut for:
115
      *
Don Gagne's avatar
Don Gagne committed
116 117 118 119
      * \code
      * ColPivHouseholderQR<MatrixType> qr(matrix.rows(), matrix.cols());
      * qr.compute(matrix);
      * \endcode
120
      *
Don Gagne's avatar
Don Gagne committed
121 122
      * \sa compute()
      */
123 124
    template<typename InputType>
    explicit ColPivHouseholderQR(const EigenBase<InputType>& matrix)
LM's avatar
LM committed
125
      : m_qr(matrix.rows(), matrix.cols()),
Don Gagne's avatar
Don Gagne committed
126 127
        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
        m_colsPermutation(PermIndexType(matrix.cols())),
LM's avatar
LM committed
128 129
        m_colsTranspositions(matrix.cols()),
        m_temp(matrix.cols()),
130 131
        m_colNormsUpdated(matrix.cols()),
        m_colNormsDirect(matrix.cols()),
LM's avatar
LM committed
132 133 134
        m_isInitialized(false),
        m_usePrescribedThreshold(false)
    {
135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
      compute(matrix.derived());
    }

    /** \brief Constructs a QR factorization from a given matrix
      *
      * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when \c MatrixType is a Eigen::Ref.
      *
      * \sa ColPivHouseholderQR(const EigenBase&)
      */
    template<typename InputType>
    explicit ColPivHouseholderQR(EigenBase<InputType>& matrix)
      : m_qr(matrix.derived()),
        m_hCoeffs((std::min)(matrix.rows(),matrix.cols())),
        m_colsPermutation(PermIndexType(matrix.cols())),
        m_colsTranspositions(matrix.cols()),
        m_temp(matrix.cols()),
        m_colNormsUpdated(matrix.cols()),
        m_colNormsDirect(matrix.cols()),
        m_isInitialized(false),
        m_usePrescribedThreshold(false)
    {
      computeInPlace();
LM's avatar
LM committed
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173
    }

    /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
      * *this is the QR decomposition, if any exists.
      *
      * \param b the right-hand-side of the equation to solve.
      *
      * \returns a solution.
      *
      * \note_about_checking_solutions
      *
      * \note_about_arbitrary_choice_of_solution
      *
      * Example: \include ColPivHouseholderQR_solve.cpp
      * Output: \verbinclude ColPivHouseholderQR_solve.out
      */
    template<typename Rhs>
174
    inline const Solve<ColPivHouseholderQR, Rhs>
LM's avatar
LM committed
175 176 177
    solve(const MatrixBase<Rhs>& b) const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
178
      return Solve<ColPivHouseholderQR, Rhs>(*this, b.derived());
LM's avatar
LM committed
179 180
    }

181 182
    HouseholderSequenceType householderQ() const;
    HouseholderSequenceType matrixQ() const
Don Gagne's avatar
Don Gagne committed
183
    {
184
      return householderQ();
Don Gagne's avatar
Don Gagne committed
185
    }
LM's avatar
LM committed
186 187 188 189 190 191 192 193

    /** \returns a reference to the matrix where the Householder QR decomposition is stored
      */
    const MatrixType& matrixQR() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return m_qr;
    }
194 195 196

    /** \returns a reference to the matrix where the result Householder QR is stored
     * \warning The strict lower part of this matrix contains internal values.
Don Gagne's avatar
Don Gagne committed
197 198
     * Only the upper triangular part should be referenced. To get it, use
     * \code matrixR().template triangularView<Upper>() \endcode
199 200 201
     * For rank-deficient matrices, use
     * \code
     * matrixR().topLeftCorner(rank(), rank()).template triangularView<Upper>()
Don Gagne's avatar
Don Gagne committed
202 203 204 205 206 207 208
     * \endcode
     */
    const MatrixType& matrixR() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return m_qr;
    }
209 210 211

    template<typename InputType>
    ColPivHouseholderQR& compute(const EigenBase<InputType>& matrix);
LM's avatar
LM committed
212

Don Gagne's avatar
Don Gagne committed
213
    /** \returns a const reference to the column permutation matrix */
LM's avatar
LM committed
214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256
    const PermutationType& colsPermutation() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return m_colsPermutation;
    }

    /** \returns the absolute value of the determinant of the matrix of which
      * *this is the QR decomposition. It has only linear complexity
      * (that is, O(n) where n is the dimension of the square matrix)
      * as the QR decomposition has already been computed.
      *
      * \note This is only for square matrices.
      *
      * \warning a determinant can be very big or small, so for matrices
      * of large enough dimension, there is a risk of overflow/underflow.
      * One way to work around that is to use logAbsDeterminant() instead.
      *
      * \sa logAbsDeterminant(), MatrixBase::determinant()
      */
    typename MatrixType::RealScalar absDeterminant() const;

    /** \returns the natural log of the absolute value of the determinant of the matrix of which
      * *this is the QR decomposition. It has only linear complexity
      * (that is, O(n) where n is the dimension of the square matrix)
      * as the QR decomposition has already been computed.
      *
      * \note This is only for square matrices.
      *
      * \note This method is useful to work around the risk of overflow/underflow that's inherent
      * to determinant computation.
      *
      * \sa absDeterminant(), MatrixBase::determinant()
      */
    typename MatrixType::RealScalar logAbsDeterminant() const;

    /** \returns the rank of the matrix of which *this is the QR decomposition.
      *
      * \note This method has to determine which pivots should be considered nonzero.
      *       For that, it uses the threshold value that you can control by calling
      *       setThreshold(const RealScalar&).
      */
    inline Index rank() const
    {
Don Gagne's avatar
Don Gagne committed
257
      using std::abs;
LM's avatar
LM committed
258
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
Don Gagne's avatar
Don Gagne committed
259
      RealScalar premultiplied_threshold = abs(m_maxpivot) * threshold();
LM's avatar
LM committed
260 261
      Index result = 0;
      for(Index i = 0; i < m_nonzero_pivots; ++i)
Don Gagne's avatar
Don Gagne committed
262
        result += (abs(m_qr.coeff(i,i)) > premultiplied_threshold);
LM's avatar
LM committed
263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320
      return result;
    }

    /** \returns the dimension of the kernel of the matrix of which *this is the QR decomposition.
      *
      * \note This method has to determine which pivots should be considered nonzero.
      *       For that, it uses the threshold value that you can control by calling
      *       setThreshold(const RealScalar&).
      */
    inline Index dimensionOfKernel() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return cols() - rank();
    }

    /** \returns true if the matrix of which *this is the QR decomposition represents an injective
      *          linear map, i.e. has trivial kernel; false otherwise.
      *
      * \note This method has to determine which pivots should be considered nonzero.
      *       For that, it uses the threshold value that you can control by calling
      *       setThreshold(const RealScalar&).
      */
    inline bool isInjective() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return rank() == cols();
    }

    /** \returns true if the matrix of which *this is the QR decomposition represents a surjective
      *          linear map; false otherwise.
      *
      * \note This method has to determine which pivots should be considered nonzero.
      *       For that, it uses the threshold value that you can control by calling
      *       setThreshold(const RealScalar&).
      */
    inline bool isSurjective() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return rank() == rows();
    }

    /** \returns true if the matrix of which *this is the QR decomposition is invertible.
      *
      * \note This method has to determine which pivots should be considered nonzero.
      *       For that, it uses the threshold value that you can control by calling
      *       setThreshold(const RealScalar&).
      */
    inline bool isInvertible() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return isInjective() && isSurjective();
    }

    /** \returns the inverse of the matrix of which *this is the QR decomposition.
      *
      * \note If this matrix is not invertible, the returned matrix has undefined coefficients.
      *       Use isInvertible() to first determine whether this matrix is invertible.
      */
321
    inline const Inverse<ColPivHouseholderQR> inverse() const
LM's avatar
LM committed
322 323
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
324
      return Inverse<ColPivHouseholderQR>(*this);
LM's avatar
LM committed
325 326 327 328
    }

    inline Index rows() const { return m_qr.rows(); }
    inline Index cols() const { return m_qr.cols(); }
329

Don Gagne's avatar
Don Gagne committed
330
    /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
331
      *
Don Gagne's avatar
Don Gagne committed
332 333
      * For advanced uses only.
      */
LM's avatar
LM committed
334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379 380 381 382 383
    const HCoeffsType& hCoeffs() const { return m_hCoeffs; }

    /** Allows to prescribe a threshold to be used by certain methods, such as rank(),
      * who need to determine when pivots are to be considered nonzero. This is not used for the
      * QR decomposition itself.
      *
      * When it needs to get the threshold value, Eigen calls threshold(). By default, this
      * uses a formula to automatically determine a reasonable threshold.
      * Once you have called the present method setThreshold(const RealScalar&),
      * your value is used instead.
      *
      * \param threshold The new value to use as the threshold.
      *
      * A pivot will be considered nonzero if its absolute value is strictly greater than
      *  \f$ \vert pivot \vert \leqslant threshold \times \vert maxpivot \vert \f$
      * where maxpivot is the biggest pivot.
      *
      * If you want to come back to the default behavior, call setThreshold(Default_t)
      */
    ColPivHouseholderQR& setThreshold(const RealScalar& threshold)
    {
      m_usePrescribedThreshold = true;
      m_prescribedThreshold = threshold;
      return *this;
    }

    /** Allows to come back to the default behavior, letting Eigen use its default formula for
      * determining the threshold.
      *
      * You should pass the special object Eigen::Default as parameter here.
      * \code qr.setThreshold(Eigen::Default); \endcode
      *
      * See the documentation of setThreshold(const RealScalar&).
      */
    ColPivHouseholderQR& setThreshold(Default_t)
    {
      m_usePrescribedThreshold = false;
      return *this;
    }

    /** Returns the threshold that will be used by certain methods such as rank().
      *
      * See the documentation of setThreshold(const RealScalar&).
      */
    RealScalar threshold() const
    {
      eigen_assert(m_isInitialized || m_usePrescribedThreshold);
      return m_usePrescribedThreshold ? m_prescribedThreshold
      // this formula comes from experimenting (see "LU precision tuning" thread on the list)
      // and turns out to be identical to Higham's formula used already in LDLt.
384
                                      : NumTraits<Scalar>::epsilon() * RealScalar(m_qr.diagonalSize());
LM's avatar
LM committed
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403
    }

    /** \returns the number of nonzero pivots in the QR decomposition.
      * Here nonzero is meant in the exact sense, not in a fuzzy sense.
      * So that notion isn't really intrinsically interesting, but it is
      * still useful when implementing algorithms.
      *
      * \sa rank()
      */
    inline Index nonzeroPivots() const
    {
      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
      return m_nonzero_pivots;
    }

    /** \returns the absolute value of the biggest pivot, i.e. the biggest
      *          diagonal coefficient of R.
      */
    RealScalar maxPivot() const { return m_maxpivot; }
404

Don Gagne's avatar
Don Gagne committed
405 406
    /** \brief Reports whether the QR factorization was succesful.
      *
407
      * \note This function always returns \c Success. It is provided for compatibility
Don Gagne's avatar
Don Gagne committed
408
      * with other factorization routines.
409
      * \returns \c Success
Don Gagne's avatar
Don Gagne committed
410 411 412 413 414 415
      */
    ComputationInfo info() const
    {
      eigen_assert(m_isInitialized && "Decomposition is not initialized.");
      return Success;
    }
LM's avatar
LM committed
416

417 418 419 420 421 422
    #ifndef EIGEN_PARSED_BY_DOXYGEN
    template<typename RhsType, typename DstType>
    EIGEN_DEVICE_FUNC
    void _solve_impl(const RhsType &rhs, DstType &dst) const;
    #endif

LM's avatar
LM committed
423
  protected:
424 425 426

    friend class CompleteOrthogonalDecomposition<MatrixType>;

427 428 429 430
    static void check_template_parameters()
    {
      EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
    }
431 432 433

    void computeInPlace();

LM's avatar
LM committed
434 435 436 437 438
    MatrixType m_qr;
    HCoeffsType m_hCoeffs;
    PermutationType m_colsPermutation;
    IntRowVectorType m_colsTranspositions;
    RowVectorType m_temp;
439 440
    RealRowVectorType m_colNormsUpdated;
    RealRowVectorType m_colNormsDirect;
LM's avatar
LM committed
441 442 443 444 445 446 447 448 449
    bool m_isInitialized, m_usePrescribedThreshold;
    RealScalar m_prescribedThreshold, m_maxpivot;
    Index m_nonzero_pivots;
    Index m_det_pq;
};

template<typename MatrixType>
typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::absDeterminant() const
{
Don Gagne's avatar
Don Gagne committed
450
  using std::abs;
LM's avatar
LM committed
451 452
  eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
  eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
Don Gagne's avatar
Don Gagne committed
453
  return abs(m_qr.diagonal().prod());
LM's avatar
LM committed
454 455 456 457 458 459 460 461 462 463
}

template<typename MatrixType>
typename MatrixType::RealScalar ColPivHouseholderQR<MatrixType>::logAbsDeterminant() const
{
  eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
  eigen_assert(m_qr.rows() == m_qr.cols() && "You can't take the determinant of a non-square matrix!");
  return m_qr.diagonal().cwiseAbs().array().log().sum();
}

Don Gagne's avatar
Don Gagne committed
464 465 466 467 468 469
/** Performs the QR factorization of the given matrix \a matrix. The result of
  * the factorization is stored into \c *this, and a reference to \c *this
  * is returned.
  *
  * \sa class ColPivHouseholderQR, ColPivHouseholderQR(const MatrixType&)
  */
LM's avatar
LM committed
470
template<typename MatrixType>
471 472 473 474 475 476 477 478 479 480
template<typename InputType>
ColPivHouseholderQR<MatrixType>& ColPivHouseholderQR<MatrixType>::compute(const EigenBase<InputType>& matrix)
{
  m_qr = matrix.derived();
  computeInPlace();
  return *this;
}

template<typename MatrixType>
void ColPivHouseholderQR<MatrixType>::computeInPlace()
LM's avatar
LM committed
481
{
482
  check_template_parameters();
483

Don Gagne's avatar
Don Gagne committed
484
  // the column permutation is stored as int indices, so just to be sure:
485 486 487 488 489 490 491
  eigen_assert(m_qr.cols()<=NumTraits<int>::highest());

  using std::abs;

  Index rows = m_qr.rows();
  Index cols = m_qr.cols();
  Index size = m_qr.diagonalSize();
LM's avatar
LM committed
492 493 494 495 496

  m_hCoeffs.resize(size);

  m_temp.resize(cols);

497
  m_colsTranspositions.resize(m_qr.cols());
LM's avatar
LM committed
498 499
  Index number_of_transpositions = 0;

500 501 502 503 504 505 506 507
  m_colNormsUpdated.resize(cols);
  m_colNormsDirect.resize(cols);
  for (Index k = 0; k < cols; ++k) {
    // colNormsDirect(k) caches the most recent directly computed norm of
    // column k.
    m_colNormsDirect.coeffRef(k) = m_qr.col(k).norm();
    m_colNormsUpdated.coeffRef(k) = m_colNormsDirect.coeffRef(k);
  }
LM's avatar
LM committed
508

509 510
  RealScalar threshold_helper =  numext::abs2<RealScalar>(m_colNormsUpdated.maxCoeff() * NumTraits<RealScalar>::epsilon()) / RealScalar(rows);
  RealScalar norm_downdate_threshold = numext::sqrt(NumTraits<RealScalar>::epsilon());
LM's avatar
LM committed
511 512 513 514 515 516

  m_nonzero_pivots = size; // the generic case is that in which all pivots are nonzero (invertible case)
  m_maxpivot = RealScalar(0);

  for(Index k = 0; k < size; ++k)
  {
517
    // first, we look up in our table m_colNormsUpdated which column has the biggest norm
LM's avatar
LM committed
518
    Index biggest_col_index;
519
    RealScalar biggest_col_sq_norm = numext::abs2(m_colNormsUpdated.tail(cols-k).maxCoeff(&biggest_col_index));
LM's avatar
LM committed
520 521
    biggest_col_index += k;

522 523 524
    // Track the number of meaningful pivots but do not stop the decomposition to make
    // sure that the initial matrix is properly reproduced. See bug 941.
    if(m_nonzero_pivots==size && biggest_col_sq_norm < threshold_helper * RealScalar(rows-k))
LM's avatar
LM committed
525 526 527 528 529 530
      m_nonzero_pivots = k;

    // apply the transposition to the columns
    m_colsTranspositions.coeffRef(k) = biggest_col_index;
    if(k != biggest_col_index) {
      m_qr.col(k).swap(m_qr.col(biggest_col_index));
531 532
      std::swap(m_colNormsUpdated.coeffRef(k), m_colNormsUpdated.coeffRef(biggest_col_index));
      std::swap(m_colNormsDirect.coeffRef(k), m_colNormsDirect.coeffRef(biggest_col_index));
LM's avatar
LM committed
533 534 535 536 537 538 539 540 541 542 543
      ++number_of_transpositions;
    }

    // generate the householder vector, store it below the diagonal
    RealScalar beta;
    m_qr.col(k).tail(rows-k).makeHouseholderInPlace(m_hCoeffs.coeffRef(k), beta);

    // apply the householder transformation to the diagonal coefficient
    m_qr.coeffRef(k,k) = beta;

    // remember the maximum absolute value of diagonal coefficients
Don Gagne's avatar
Don Gagne committed
544
    if(abs(beta) > m_maxpivot) m_maxpivot = abs(beta);
LM's avatar
LM committed
545 546 547 548 549

    // apply the householder transformation
    m_qr.bottomRightCorner(rows-k, cols-k-1)
        .applyHouseholderOnTheLeft(m_qr.col(k).tail(rows-k-1), m_hCoeffs.coeffRef(k), &m_temp.coeffRef(k+1));

550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571
    // update our table of norms of the columns
    for (Index j = k + 1; j < cols; ++j) {
      // The following implements the stable norm downgrade step discussed in
      // http://www.netlib.org/lapack/lawnspdf/lawn176.pdf
      // and used in LAPACK routines xGEQPF and xGEQP3.
      // See lines 278-297 in http://www.netlib.org/lapack/explore-html/dc/df4/sgeqpf_8f_source.html
      if (m_colNormsUpdated.coeffRef(j) != RealScalar(0)) {
        RealScalar temp = abs(m_qr.coeffRef(k, j)) / m_colNormsUpdated.coeffRef(j);
        temp = (RealScalar(1) + temp) * (RealScalar(1) - temp);
        temp = temp <  RealScalar(0) ? RealScalar(0) : temp;
        RealScalar temp2 = temp * numext::abs2<RealScalar>(m_colNormsUpdated.coeffRef(j) /
                                                           m_colNormsDirect.coeffRef(j));
        if (temp2 <= norm_downdate_threshold) {
          // The updated norm has become too inaccurate so re-compute the column
          // norm directly.
          m_colNormsDirect.coeffRef(j) = m_qr.col(j).tail(rows - k - 1).norm();
          m_colNormsUpdated.coeffRef(j) = m_colNormsDirect.coeffRef(j);
        } else {
          m_colNormsUpdated.coeffRef(j) *= numext::sqrt(temp);
        }
      }
    }
LM's avatar
LM committed
572 573
  }

Don Gagne's avatar
Don Gagne committed
574
  m_colsPermutation.setIdentity(PermIndexType(cols));
575
  for(PermIndexType k = 0; k < size/*m_nonzero_pivots*/; ++k)
Don Gagne's avatar
Don Gagne committed
576
    m_colsPermutation.applyTranspositionOnTheRight(k, PermIndexType(m_colsTranspositions.coeff(k)));
LM's avatar
LM committed
577 578 579 580 581

  m_det_pq = (number_of_transpositions%2) ? -1 : 1;
  m_isInitialized = true;
}

582 583 584 585
#ifndef EIGEN_PARSED_BY_DOXYGEN
template<typename _MatrixType>
template<typename RhsType, typename DstType>
void ColPivHouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
LM's avatar
LM committed
586
{
587 588 589
  eigen_assert(rhs.rows() == rows());

  const Index nonzero_pivots = nonzeroPivots();
LM's avatar
LM committed
590

591
  if(nonzero_pivots == 0)
LM's avatar
LM committed
592
  {
593 594 595
    dst.setZero();
    return;
  }
LM's avatar
LM committed
596

597
  typename RhsType::PlainObject c(rhs);
LM's avatar
LM committed
598

599 600 601 602 603
  // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T
  c.applyOnTheLeft(householderSequence(m_qr, m_hCoeffs)
                    .setLength(nonzero_pivots)
                    .transpose()
    );
LM's avatar
LM committed
604

605 606 607
  m_qr.topLeftCorner(nonzero_pivots, nonzero_pivots)
      .template triangularView<Upper>()
      .solveInPlace(c.topRows(nonzero_pivots));
LM's avatar
LM committed
608

609 610 611 612
  for(Index i = 0; i < nonzero_pivots; ++i) dst.row(m_colsPermutation.indices().coeff(i)) = c.row(i);
  for(Index i = nonzero_pivots; i < cols(); ++i) dst.row(m_colsPermutation.indices().coeff(i)).setZero();
}
#endif
LM's avatar
LM committed
613

614
namespace internal {
LM's avatar
LM committed
615

616 617 618 619 620 621 622 623
template<typename DstXprType, typename MatrixType>
struct Assignment<DstXprType, Inverse<ColPivHouseholderQR<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename ColPivHouseholderQR<MatrixType>::Scalar>, Dense2Dense>
{
  typedef ColPivHouseholderQR<MatrixType> QrType;
  typedef Inverse<QrType> SrcXprType;
  static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename QrType::Scalar> &)
  {
    dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.cols()));
LM's avatar
LM committed
624 625 626 627 628
  }
};

} // end namespace internal

629 630 631
/** \returns the matrix Q as a sequence of householder transformations.
  * You can extract the meaningful part only by using:
  * \code qr.householderQ().setLength(qr.nonzeroPivots()) \endcode*/
LM's avatar
LM committed
632 633 634 635 636
template<typename MatrixType>
typename ColPivHouseholderQR<MatrixType>::HouseholderSequenceType ColPivHouseholderQR<MatrixType>
  ::householderQ() const
{
  eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
637
  return HouseholderSequenceType(m_qr, m_hCoeffs.conjugate());
LM's avatar
LM committed
638 639 640 641 642 643 644 645 646 647 648 649 650
}

/** \return the column-pivoting Householder QR decomposition of \c *this.
  *
  * \sa class ColPivHouseholderQR
  */
template<typename Derived>
const ColPivHouseholderQR<typename MatrixBase<Derived>::PlainObject>
MatrixBase<Derived>::colPivHouseholderQr() const
{
  return ColPivHouseholderQR<PlainObject>(eval());
}

Don Gagne's avatar
Don Gagne committed
651
} // end namespace Eigen
LM's avatar
LM committed
652 653

#endif // EIGEN_COLPIVOTINGHOUSEHOLDERQR_H