[136] | 1 | // This file is part of Eigen, a lightweight C++ template library
|
---|
| 2 | // for linear algebra.
|
---|
| 3 | //
|
---|
| 4 | // Copyright (C) 2012-2013 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
|
---|
| 5 | // Copyright (C) 2012-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
|
---|
| 6 | //
|
---|
| 7 | // This Source Code Form is subject to the terms of the Mozilla
|
---|
| 8 | // Public License v. 2.0. If a copy of the MPL was not distributed
|
---|
| 9 | // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
|
---|
| 10 |
|
---|
| 11 | #ifndef EIGEN_SPARSE_QR_H
|
---|
| 12 | #define EIGEN_SPARSE_QR_H
|
---|
| 13 |
|
---|
| 14 | namespace Eigen {
|
---|
| 15 |
|
---|
| 16 | template<typename MatrixType, typename OrderingType> class SparseQR;
|
---|
| 17 | template<typename SparseQRType> struct SparseQRMatrixQReturnType;
|
---|
| 18 | template<typename SparseQRType> struct SparseQRMatrixQTransposeReturnType;
|
---|
| 19 | template<typename SparseQRType, typename Derived> struct SparseQR_QProduct;
|
---|
| 20 | namespace internal {
|
---|
| 21 | template <typename SparseQRType> struct traits<SparseQRMatrixQReturnType<SparseQRType> >
|
---|
| 22 | {
|
---|
| 23 | typedef typename SparseQRType::MatrixType ReturnType;
|
---|
| 24 | typedef typename ReturnType::Index Index;
|
---|
| 25 | typedef typename ReturnType::StorageKind StorageKind;
|
---|
| 26 | };
|
---|
| 27 | template <typename SparseQRType> struct traits<SparseQRMatrixQTransposeReturnType<SparseQRType> >
|
---|
| 28 | {
|
---|
| 29 | typedef typename SparseQRType::MatrixType ReturnType;
|
---|
| 30 | };
|
---|
| 31 | template <typename SparseQRType, typename Derived> struct traits<SparseQR_QProduct<SparseQRType, Derived> >
|
---|
| 32 | {
|
---|
| 33 | typedef typename Derived::PlainObject ReturnType;
|
---|
| 34 | };
|
---|
| 35 | } // End namespace internal
|
---|
| 36 |
|
---|
| 37 | /**
|
---|
| 38 | * \ingroup SparseQR_Module
|
---|
| 39 | * \class SparseQR
|
---|
| 40 | * \brief Sparse left-looking rank-revealing QR factorization
|
---|
| 41 | *
|
---|
| 42 | * This class implements a left-looking rank-revealing QR decomposition
|
---|
| 43 | * of sparse matrices. When a column has a norm less than a given tolerance
|
---|
| 44 | * it is implicitly permuted to the end. The QR factorization thus obtained is
|
---|
| 45 | * given by A*P = Q*R where R is upper triangular or trapezoidal.
|
---|
| 46 | *
|
---|
| 47 | * P is the column permutation which is the product of the fill-reducing and the
|
---|
| 48 | * rank-revealing permutations. Use colsPermutation() to get it.
|
---|
| 49 | *
|
---|
| 50 | * Q is the orthogonal matrix represented as products of Householder reflectors.
|
---|
| 51 | * Use matrixQ() to get an expression and matrixQ().transpose() to get the transpose.
|
---|
| 52 | * You can then apply it to a vector.
|
---|
| 53 | *
|
---|
| 54 | * R is the sparse triangular or trapezoidal matrix. The later occurs when A is rank-deficient.
|
---|
| 55 | * matrixR().topLeftCorner(rank(), rank()) always returns a triangular factor of full rank.
|
---|
| 56 | *
|
---|
| 57 | * \tparam _MatrixType The type of the sparse matrix A, must be a column-major SparseMatrix<>
|
---|
| 58 | * \tparam _OrderingType The fill-reducing ordering method. See the \link OrderingMethods_Module
|
---|
| 59 | * OrderingMethods \endlink module for the list of built-in and external ordering methods.
|
---|
| 60 | *
|
---|
| 61 | * \warning The input sparse matrix A must be in compressed mode (see SparseMatrix::makeCompressed()).
|
---|
| 62 | *
|
---|
| 63 | */
|
---|
| 64 | template<typename _MatrixType, typename _OrderingType>
|
---|
| 65 | class SparseQR
|
---|
| 66 | {
|
---|
| 67 | public:
|
---|
| 68 | typedef _MatrixType MatrixType;
|
---|
| 69 | typedef _OrderingType OrderingType;
|
---|
| 70 | typedef typename MatrixType::Scalar Scalar;
|
---|
| 71 | typedef typename MatrixType::RealScalar RealScalar;
|
---|
| 72 | typedef typename MatrixType::Index Index;
|
---|
| 73 | typedef SparseMatrix<Scalar,ColMajor,Index> QRMatrixType;
|
---|
| 74 | typedef Matrix<Index, Dynamic, 1> IndexVector;
|
---|
| 75 | typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
|
---|
| 76 | typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
|
---|
| 77 | public:
|
---|
| 78 | SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
|
---|
| 79 | { }
|
---|
| 80 |
|
---|
| 81 | /** Construct a QR factorization of the matrix \a mat.
|
---|
| 82 | *
|
---|
| 83 | * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
|
---|
| 84 | *
|
---|
| 85 | * \sa compute()
|
---|
| 86 | */
|
---|
| 87 | SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
|
---|
| 88 | {
|
---|
| 89 | compute(mat);
|
---|
| 90 | }
|
---|
| 91 |
|
---|
| 92 | /** Computes the QR factorization of the sparse matrix \a mat.
|
---|
| 93 | *
|
---|
| 94 | * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
|
---|
| 95 | *
|
---|
| 96 | * \sa analyzePattern(), factorize()
|
---|
| 97 | */
|
---|
| 98 | void compute(const MatrixType& mat)
|
---|
| 99 | {
|
---|
| 100 | analyzePattern(mat);
|
---|
| 101 | factorize(mat);
|
---|
| 102 | }
|
---|
| 103 | void analyzePattern(const MatrixType& mat);
|
---|
| 104 | void factorize(const MatrixType& mat);
|
---|
| 105 |
|
---|
| 106 | /** \returns the number of rows of the represented matrix.
|
---|
| 107 | */
|
---|
| 108 | inline Index rows() const { return m_pmat.rows(); }
|
---|
| 109 |
|
---|
| 110 | /** \returns the number of columns of the represented matrix.
|
---|
| 111 | */
|
---|
| 112 | inline Index cols() const { return m_pmat.cols();}
|
---|
| 113 |
|
---|
| 114 | /** \returns a const reference to the \b sparse upper triangular matrix R of the QR factorization.
|
---|
| 115 | */
|
---|
| 116 | const QRMatrixType& matrixR() const { return m_R; }
|
---|
| 117 |
|
---|
| 118 | /** \returns the number of non linearly dependent columns as determined by the pivoting threshold.
|
---|
| 119 | *
|
---|
| 120 | * \sa setPivotThreshold()
|
---|
| 121 | */
|
---|
| 122 | Index rank() const
|
---|
| 123 | {
|
---|
| 124 | eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
|
---|
| 125 | return m_nonzeropivots;
|
---|
| 126 | }
|
---|
| 127 |
|
---|
| 128 | /** \returns an expression of the matrix Q as products of sparse Householder reflectors.
|
---|
| 129 | * The common usage of this function is to apply it to a dense matrix or vector
|
---|
| 130 | * \code
|
---|
| 131 | * VectorXd B1, B2;
|
---|
| 132 | * // Initialize B1
|
---|
| 133 | * B2 = matrixQ() * B1;
|
---|
| 134 | * \endcode
|
---|
| 135 | *
|
---|
| 136 | * To get a plain SparseMatrix representation of Q:
|
---|
| 137 | * \code
|
---|
| 138 | * SparseMatrix<double> Q;
|
---|
| 139 | * Q = SparseQR<SparseMatrix<double> >(A).matrixQ();
|
---|
| 140 | * \endcode
|
---|
| 141 | * Internally, this call simply performs a sparse product between the matrix Q
|
---|
| 142 | * and a sparse identity matrix. However, due to the fact that the sparse
|
---|
| 143 | * reflectors are stored unsorted, two transpositions are needed to sort
|
---|
| 144 | * them before performing the product.
|
---|
| 145 | */
|
---|
| 146 | SparseQRMatrixQReturnType<SparseQR> matrixQ() const
|
---|
| 147 | { return SparseQRMatrixQReturnType<SparseQR>(*this); }
|
---|
| 148 |
|
---|
| 149 | /** \returns a const reference to the column permutation P that was applied to A such that A*P = Q*R
|
---|
| 150 | * It is the combination of the fill-in reducing permutation and numerical column pivoting.
|
---|
| 151 | */
|
---|
| 152 | const PermutationType& colsPermutation() const
|
---|
| 153 | {
|
---|
| 154 | eigen_assert(m_isInitialized && "Decomposition is not initialized.");
|
---|
| 155 | return m_outputPerm_c;
|
---|
| 156 | }
|
---|
| 157 |
|
---|
| 158 | /** \returns A string describing the type of error.
|
---|
| 159 | * This method is provided to ease debugging, not to handle errors.
|
---|
| 160 | */
|
---|
| 161 | std::string lastErrorMessage() const { return m_lastError; }
|
---|
| 162 |
|
---|
| 163 | /** \internal */
|
---|
| 164 | template<typename Rhs, typename Dest>
|
---|
| 165 | bool _solve(const MatrixBase<Rhs> &B, MatrixBase<Dest> &dest) const
|
---|
| 166 | {
|
---|
| 167 | eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
|
---|
| 168 | eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
|
---|
| 169 |
|
---|
| 170 | Index rank = this->rank();
|
---|
| 171 |
|
---|
| 172 | // Compute Q^T * b;
|
---|
| 173 | typename Dest::PlainObject y, b;
|
---|
| 174 | y = this->matrixQ().transpose() * B;
|
---|
| 175 | b = y;
|
---|
| 176 |
|
---|
| 177 | // Solve with the triangular matrix R
|
---|
| 178 | y.resize((std::max)(cols(),Index(y.rows())),y.cols());
|
---|
| 179 | y.topRows(rank) = this->matrixR().topLeftCorner(rank, rank).template triangularView<Upper>().solve(b.topRows(rank));
|
---|
| 180 | y.bottomRows(y.rows()-rank).setZero();
|
---|
| 181 |
|
---|
| 182 | // Apply the column permutation
|
---|
| 183 | if (m_perm_c.size()) dest = colsPermutation() * y.topRows(cols());
|
---|
| 184 | else dest = y.topRows(cols());
|
---|
| 185 |
|
---|
| 186 | m_info = Success;
|
---|
| 187 | return true;
|
---|
| 188 | }
|
---|
| 189 |
|
---|
| 190 |
|
---|
| 191 | /** Sets the threshold that is used to determine linearly dependent columns during the factorization.
|
---|
| 192 | *
|
---|
| 193 | * In practice, if during the factorization the norm of the column that has to be eliminated is below
|
---|
| 194 | * this threshold, then the entire column is treated as zero, and it is moved at the end.
|
---|
| 195 | */
|
---|
| 196 | void setPivotThreshold(const RealScalar& threshold)
|
---|
| 197 | {
|
---|
| 198 | m_useDefaultThreshold = false;
|
---|
| 199 | m_threshold = threshold;
|
---|
| 200 | }
|
---|
| 201 |
|
---|
| 202 | /** \returns the solution X of \f$ A X = B \f$ using the current decomposition of A.
|
---|
| 203 | *
|
---|
| 204 | * \sa compute()
|
---|
| 205 | */
|
---|
| 206 | template<typename Rhs>
|
---|
| 207 | inline const internal::solve_retval<SparseQR, Rhs> solve(const MatrixBase<Rhs>& B) const
|
---|
| 208 | {
|
---|
| 209 | eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
|
---|
| 210 | eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
|
---|
| 211 | return internal::solve_retval<SparseQR, Rhs>(*this, B.derived());
|
---|
| 212 | }
|
---|
| 213 | template<typename Rhs>
|
---|
| 214 | inline const internal::sparse_solve_retval<SparseQR, Rhs> solve(const SparseMatrixBase<Rhs>& B) const
|
---|
| 215 | {
|
---|
| 216 | eigen_assert(m_isInitialized && "The factorization should be called first, use compute()");
|
---|
| 217 | eigen_assert(this->rows() == B.rows() && "SparseQR::solve() : invalid number of rows in the right hand side matrix");
|
---|
| 218 | return internal::sparse_solve_retval<SparseQR, Rhs>(*this, B.derived());
|
---|
| 219 | }
|
---|
| 220 |
|
---|
| 221 | /** \brief Reports whether previous computation was successful.
|
---|
| 222 | *
|
---|
| 223 | * \returns \c Success if computation was successful,
|
---|
| 224 | * \c NumericalIssue if the QR factorization reports a numerical problem
|
---|
| 225 | * \c InvalidInput if the input matrix is invalid
|
---|
| 226 | *
|
---|
| 227 | * \sa iparm()
|
---|
| 228 | */
|
---|
| 229 | ComputationInfo info() const
|
---|
| 230 | {
|
---|
| 231 | eigen_assert(m_isInitialized && "Decomposition is not initialized.");
|
---|
| 232 | return m_info;
|
---|
| 233 | }
|
---|
| 234 |
|
---|
| 235 | protected:
|
---|
| 236 | inline void sort_matrix_Q()
|
---|
| 237 | {
|
---|
| 238 | if(this->m_isQSorted) return;
|
---|
| 239 | // The matrix Q is sorted during the transposition
|
---|
| 240 | SparseMatrix<Scalar, RowMajor, Index> mQrm(this->m_Q);
|
---|
| 241 | this->m_Q = mQrm;
|
---|
| 242 | this->m_isQSorted = true;
|
---|
| 243 | }
|
---|
| 244 |
|
---|
| 245 |
|
---|
| 246 | protected:
|
---|
| 247 | bool m_isInitialized;
|
---|
| 248 | bool m_analysisIsok;
|
---|
| 249 | bool m_factorizationIsok;
|
---|
| 250 | mutable ComputationInfo m_info;
|
---|
| 251 | std::string m_lastError;
|
---|
| 252 | QRMatrixType m_pmat; // Temporary matrix
|
---|
| 253 | QRMatrixType m_R; // The triangular factor matrix
|
---|
| 254 | QRMatrixType m_Q; // The orthogonal reflectors
|
---|
| 255 | ScalarVector m_hcoeffs; // The Householder coefficients
|
---|
| 256 | PermutationType m_perm_c; // Fill-reducing Column permutation
|
---|
| 257 | PermutationType m_pivotperm; // The permutation for rank revealing
|
---|
| 258 | PermutationType m_outputPerm_c; // The final column permutation
|
---|
| 259 | RealScalar m_threshold; // Threshold to determine null Householder reflections
|
---|
| 260 | bool m_useDefaultThreshold; // Use default threshold
|
---|
| 261 | Index m_nonzeropivots; // Number of non zero pivots found
|
---|
| 262 | IndexVector m_etree; // Column elimination tree
|
---|
| 263 | IndexVector m_firstRowElt; // First element in each row
|
---|
| 264 | bool m_isQSorted; // whether Q is sorted or not
|
---|
| 265 | bool m_isEtreeOk; // whether the elimination tree match the initial input matrix
|
---|
| 266 |
|
---|
| 267 | template <typename, typename > friend struct SparseQR_QProduct;
|
---|
| 268 | template <typename > friend struct SparseQRMatrixQReturnType;
|
---|
| 269 |
|
---|
| 270 | };
|
---|
| 271 |
|
---|
| 272 | /** \brief Preprocessing step of a QR factorization
|
---|
| 273 | *
|
---|
| 274 | * \warning The matrix \a mat must be in compressed mode (see SparseMatrix::makeCompressed()).
|
---|
| 275 | *
|
---|
| 276 | * In this step, the fill-reducing permutation is computed and applied to the columns of A
|
---|
| 277 | * and the column elimination tree is computed as well. Only the sparsity pattern of \a mat is exploited.
|
---|
| 278 | *
|
---|
| 279 | * \note In this step it is assumed that there is no empty row in the matrix \a mat.
|
---|
| 280 | */
|
---|
| 281 | template <typename MatrixType, typename OrderingType>
|
---|
| 282 | void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
|
---|
| 283 | {
|
---|
| 284 | eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
|
---|
| 285 | // Copy to a column major matrix if the input is rowmajor
|
---|
| 286 | typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
|
---|
| 287 | // Compute the column fill reducing ordering
|
---|
| 288 | OrderingType ord;
|
---|
| 289 | ord(matCpy, m_perm_c);
|
---|
| 290 | Index n = mat.cols();
|
---|
| 291 | Index m = mat.rows();
|
---|
| 292 | Index diagSize = (std::min)(m,n);
|
---|
| 293 |
|
---|
| 294 | if (!m_perm_c.size())
|
---|
| 295 | {
|
---|
| 296 | m_perm_c.resize(n);
|
---|
| 297 | m_perm_c.indices().setLinSpaced(n, 0,n-1);
|
---|
| 298 | }
|
---|
| 299 |
|
---|
| 300 | // Compute the column elimination tree of the permuted matrix
|
---|
| 301 | m_outputPerm_c = m_perm_c.inverse();
|
---|
| 302 | internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
|
---|
| 303 | m_isEtreeOk = true;
|
---|
| 304 |
|
---|
| 305 | m_R.resize(m, n);
|
---|
| 306 | m_Q.resize(m, diagSize);
|
---|
| 307 |
|
---|
| 308 | // Allocate space for nonzero elements : rough estimation
|
---|
| 309 | m_R.reserve(2*mat.nonZeros()); //FIXME Get a more accurate estimation through symbolic factorization with the etree
|
---|
| 310 | m_Q.reserve(2*mat.nonZeros());
|
---|
| 311 | m_hcoeffs.resize(diagSize);
|
---|
| 312 | m_analysisIsok = true;
|
---|
| 313 | }
|
---|
| 314 |
|
---|
| 315 | /** \brief Performs the numerical QR factorization of the input matrix
|
---|
| 316 | *
|
---|
| 317 | * The function SparseQR::analyzePattern(const MatrixType&) must have been called beforehand with
|
---|
| 318 | * a matrix having the same sparsity pattern than \a mat.
|
---|
| 319 | *
|
---|
| 320 | * \param mat The sparse column-major matrix
|
---|
| 321 | */
|
---|
| 322 | template <typename MatrixType, typename OrderingType>
|
---|
| 323 | void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
|
---|
| 324 | {
|
---|
| 325 | using std::abs;
|
---|
| 326 | using std::max;
|
---|
| 327 |
|
---|
| 328 | eigen_assert(m_analysisIsok && "analyzePattern() should be called before this step");
|
---|
| 329 | Index m = mat.rows();
|
---|
| 330 | Index n = mat.cols();
|
---|
| 331 | Index diagSize = (std::min)(m,n);
|
---|
| 332 | IndexVector mark((std::max)(m,n)); mark.setConstant(-1); // Record the visited nodes
|
---|
| 333 | IndexVector Ridx(n), Qidx(m); // Store temporarily the row indexes for the current column of R and Q
|
---|
| 334 | Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q
|
---|
| 335 | ScalarVector tval(m); // The dense vector used to compute the current column
|
---|
| 336 | RealScalar pivotThreshold = m_threshold;
|
---|
| 337 |
|
---|
| 338 | m_R.setZero();
|
---|
| 339 | m_Q.setZero();
|
---|
| 340 | m_pmat = mat;
|
---|
| 341 | if(!m_isEtreeOk)
|
---|
| 342 | {
|
---|
| 343 | m_outputPerm_c = m_perm_c.inverse();
|
---|
| 344 | internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
|
---|
| 345 | m_isEtreeOk = true;
|
---|
| 346 | }
|
---|
| 347 |
|
---|
| 348 | m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
|
---|
| 349 |
|
---|
| 350 | // Apply the fill-in reducing permutation lazily:
|
---|
| 351 | {
|
---|
| 352 | // If the input is row major, copy the original column indices,
|
---|
| 353 | // otherwise directly use the input matrix
|
---|
| 354 | //
|
---|
| 355 | IndexVector originalOuterIndicesCpy;
|
---|
| 356 | const Index *originalOuterIndices = mat.outerIndexPtr();
|
---|
| 357 | if(MatrixType::IsRowMajor)
|
---|
| 358 | {
|
---|
| 359 | originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
|
---|
| 360 | originalOuterIndices = originalOuterIndicesCpy.data();
|
---|
| 361 | }
|
---|
| 362 |
|
---|
| 363 | for (int i = 0; i < n; i++)
|
---|
| 364 | {
|
---|
| 365 | Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
|
---|
| 366 | m_pmat.outerIndexPtr()[p] = originalOuterIndices[i];
|
---|
| 367 | m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i];
|
---|
| 368 | }
|
---|
| 369 | }
|
---|
| 370 |
|
---|
| 371 | /* Compute the default threshold as in MatLab, see:
|
---|
| 372 | * Tim Davis, "Algorithm 915, SuiteSparseQR: Multifrontal Multithreaded Rank-Revealing
|
---|
| 373 | * Sparse QR Factorization, ACM Trans. on Math. Soft. 38(1), 2011, Page 8:3
|
---|
| 374 | */
|
---|
| 375 | if(m_useDefaultThreshold)
|
---|
| 376 | {
|
---|
| 377 | RealScalar max2Norm = 0.0;
|
---|
| 378 | for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm());
|
---|
| 379 | if(max2Norm==RealScalar(0))
|
---|
| 380 | max2Norm = RealScalar(1);
|
---|
| 381 | pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
|
---|
| 382 | }
|
---|
| 383 |
|
---|
| 384 | // Initialize the numerical permutation
|
---|
| 385 | m_pivotperm.setIdentity(n);
|
---|
| 386 |
|
---|
| 387 | Index nonzeroCol = 0; // Record the number of valid pivots
|
---|
| 388 | m_Q.startVec(0);
|
---|
| 389 |
|
---|
| 390 | // Left looking rank-revealing QR factorization: compute a column of R and Q at a time
|
---|
| 391 | for (Index col = 0; col < n; ++col)
|
---|
| 392 | {
|
---|
| 393 | mark.setConstant(-1);
|
---|
| 394 | m_R.startVec(col);
|
---|
| 395 | mark(nonzeroCol) = col;
|
---|
| 396 | Qidx(0) = nonzeroCol;
|
---|
| 397 | nzcolR = 0; nzcolQ = 1;
|
---|
| 398 | bool found_diag = nonzeroCol>=m;
|
---|
| 399 | tval.setZero();
|
---|
| 400 |
|
---|
| 401 | // Symbolic factorization: find the nonzero locations of the column k of the factors R and Q, i.e.,
|
---|
| 402 | // all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
|
---|
| 403 | // Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
|
---|
| 404 | // thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
|
---|
| 405 | for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
|
---|
| 406 | {
|
---|
| 407 | Index curIdx = nonzeroCol;
|
---|
| 408 | if(itp) curIdx = itp.row();
|
---|
| 409 | if(curIdx == nonzeroCol) found_diag = true;
|
---|
| 410 |
|
---|
| 411 | // Get the nonzeros indexes of the current column of R
|
---|
| 412 | Index st = m_firstRowElt(curIdx); // The traversal of the etree starts here
|
---|
| 413 | if (st < 0 )
|
---|
| 414 | {
|
---|
| 415 | m_lastError = "Empty row found during numerical factorization";
|
---|
| 416 | m_info = InvalidInput;
|
---|
| 417 | return;
|
---|
| 418 | }
|
---|
| 419 |
|
---|
| 420 | // Traverse the etree
|
---|
| 421 | Index bi = nzcolR;
|
---|
| 422 | for (; mark(st) != col; st = m_etree(st))
|
---|
| 423 | {
|
---|
| 424 | Ridx(nzcolR) = st; // Add this row to the list,
|
---|
| 425 | mark(st) = col; // and mark this row as visited
|
---|
| 426 | nzcolR++;
|
---|
| 427 | }
|
---|
| 428 |
|
---|
| 429 | // Reverse the list to get the topological ordering
|
---|
| 430 | Index nt = nzcolR-bi;
|
---|
| 431 | for(Index i = 0; i < nt/2; i++) std::swap(Ridx(bi+i), Ridx(nzcolR-i-1));
|
---|
| 432 |
|
---|
| 433 | // Copy the current (curIdx,pcol) value of the input matrix
|
---|
| 434 | if(itp) tval(curIdx) = itp.value();
|
---|
| 435 | else tval(curIdx) = Scalar(0);
|
---|
| 436 |
|
---|
| 437 | // Compute the pattern of Q(:,k)
|
---|
| 438 | if(curIdx > nonzeroCol && mark(curIdx) != col )
|
---|
| 439 | {
|
---|
| 440 | Qidx(nzcolQ) = curIdx; // Add this row to the pattern of Q,
|
---|
| 441 | mark(curIdx) = col; // and mark it as visited
|
---|
| 442 | nzcolQ++;
|
---|
| 443 | }
|
---|
| 444 | }
|
---|
| 445 |
|
---|
| 446 | // Browse all the indexes of R(:,col) in reverse order
|
---|
| 447 | for (Index i = nzcolR-1; i >= 0; i--)
|
---|
| 448 | {
|
---|
| 449 | Index curIdx = Ridx(i);
|
---|
| 450 |
|
---|
| 451 | // Apply the curIdx-th householder vector to the current column (temporarily stored into tval)
|
---|
| 452 | Scalar tdot(0);
|
---|
| 453 |
|
---|
| 454 | // First compute q' * tval
|
---|
| 455 | tdot = m_Q.col(curIdx).dot(tval);
|
---|
| 456 |
|
---|
| 457 | tdot *= m_hcoeffs(curIdx);
|
---|
| 458 |
|
---|
| 459 | // Then update tval = tval - q * tau
|
---|
| 460 | // FIXME: tval -= tdot * m_Q.col(curIdx) should amount to the same (need to check/add support for efficient "dense ?= sparse")
|
---|
| 461 | for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
|
---|
| 462 | tval(itq.row()) -= itq.value() * tdot;
|
---|
| 463 |
|
---|
| 464 | // Detect fill-in for the current column of Q
|
---|
| 465 | if(m_etree(Ridx(i)) == nonzeroCol)
|
---|
| 466 | {
|
---|
| 467 | for (typename QRMatrixType::InnerIterator itq(m_Q, curIdx); itq; ++itq)
|
---|
| 468 | {
|
---|
| 469 | Index iQ = itq.row();
|
---|
| 470 | if (mark(iQ) != col)
|
---|
| 471 | {
|
---|
| 472 | Qidx(nzcolQ++) = iQ; // Add this row to the pattern of Q,
|
---|
| 473 | mark(iQ) = col; // and mark it as visited
|
---|
| 474 | }
|
---|
| 475 | }
|
---|
| 476 | }
|
---|
| 477 | } // End update current column
|
---|
| 478 |
|
---|
| 479 | Scalar tau = 0;
|
---|
| 480 | RealScalar beta = 0;
|
---|
| 481 |
|
---|
| 482 | if(nonzeroCol < diagSize)
|
---|
| 483 | {
|
---|
| 484 | // Compute the Householder reflection that eliminate the current column
|
---|
| 485 | // FIXME this step should call the Householder module.
|
---|
| 486 | Scalar c0 = nzcolQ ? tval(Qidx(0)) : Scalar(0);
|
---|
| 487 |
|
---|
| 488 | // First, the squared norm of Q((col+1):m, col)
|
---|
| 489 | RealScalar sqrNorm = 0.;
|
---|
| 490 | for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
|
---|
| 491 | if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
|
---|
| 492 | {
|
---|
| 493 | beta = numext::real(c0);
|
---|
| 494 | tval(Qidx(0)) = 1;
|
---|
| 495 | }
|
---|
| 496 | else
|
---|
| 497 | {
|
---|
| 498 | using std::sqrt;
|
---|
| 499 | beta = sqrt(numext::abs2(c0) + sqrNorm);
|
---|
| 500 | if(numext::real(c0) >= RealScalar(0))
|
---|
| 501 | beta = -beta;
|
---|
| 502 | tval(Qidx(0)) = 1;
|
---|
| 503 | for (Index itq = 1; itq < nzcolQ; ++itq)
|
---|
| 504 | tval(Qidx(itq)) /= (c0 - beta);
|
---|
| 505 | tau = numext::conj((beta-c0) / beta);
|
---|
| 506 |
|
---|
| 507 | }
|
---|
| 508 | }
|
---|
| 509 |
|
---|
| 510 | // Insert values in R
|
---|
| 511 | for (Index i = nzcolR-1; i >= 0; i--)
|
---|
| 512 | {
|
---|
| 513 | Index curIdx = Ridx(i);
|
---|
| 514 | if(curIdx < nonzeroCol)
|
---|
| 515 | {
|
---|
| 516 | m_R.insertBackByOuterInnerUnordered(col, curIdx) = tval(curIdx);
|
---|
| 517 | tval(curIdx) = Scalar(0.);
|
---|
| 518 | }
|
---|
| 519 | }
|
---|
| 520 |
|
---|
| 521 | if(nonzeroCol < diagSize && abs(beta) >= pivotThreshold)
|
---|
| 522 | {
|
---|
| 523 | m_R.insertBackByOuterInner(col, nonzeroCol) = beta;
|
---|
| 524 | // The householder coefficient
|
---|
| 525 | m_hcoeffs(nonzeroCol) = tau;
|
---|
| 526 | // Record the householder reflections
|
---|
| 527 | for (Index itq = 0; itq < nzcolQ; ++itq)
|
---|
| 528 | {
|
---|
| 529 | Index iQ = Qidx(itq);
|
---|
| 530 | m_Q.insertBackByOuterInnerUnordered(nonzeroCol,iQ) = tval(iQ);
|
---|
| 531 | tval(iQ) = Scalar(0.);
|
---|
| 532 | }
|
---|
| 533 | nonzeroCol++;
|
---|
| 534 | if(nonzeroCol<diagSize)
|
---|
| 535 | m_Q.startVec(nonzeroCol);
|
---|
| 536 | }
|
---|
| 537 | else
|
---|
| 538 | {
|
---|
| 539 | // Zero pivot found: move implicitly this column to the end
|
---|
| 540 | for (Index j = nonzeroCol; j < n-1; j++)
|
---|
| 541 | std::swap(m_pivotperm.indices()(j), m_pivotperm.indices()[j+1]);
|
---|
| 542 |
|
---|
| 543 | // Recompute the column elimination tree
|
---|
| 544 | internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
|
---|
| 545 | m_isEtreeOk = false;
|
---|
| 546 | }
|
---|
| 547 | }
|
---|
| 548 |
|
---|
| 549 | m_hcoeffs.tail(diagSize-nonzeroCol).setZero();
|
---|
| 550 |
|
---|
| 551 | // Finalize the column pointers of the sparse matrices R and Q
|
---|
| 552 | m_Q.finalize();
|
---|
| 553 | m_Q.makeCompressed();
|
---|
| 554 | m_R.finalize();
|
---|
| 555 | m_R.makeCompressed();
|
---|
| 556 | m_isQSorted = false;
|
---|
| 557 |
|
---|
| 558 | m_nonzeropivots = nonzeroCol;
|
---|
| 559 |
|
---|
| 560 | if(nonzeroCol<n)
|
---|
| 561 | {
|
---|
| 562 | // Permute the triangular factor to put the 'dead' columns to the end
|
---|
| 563 | QRMatrixType tempR(m_R);
|
---|
| 564 | m_R = tempR * m_pivotperm;
|
---|
| 565 |
|
---|
| 566 | // Update the column permutation
|
---|
| 567 | m_outputPerm_c = m_outputPerm_c * m_pivotperm;
|
---|
| 568 | }
|
---|
| 569 |
|
---|
| 570 | m_isInitialized = true;
|
---|
| 571 | m_factorizationIsok = true;
|
---|
| 572 | m_info = Success;
|
---|
| 573 | }
|
---|
| 574 |
|
---|
| 575 | namespace internal {
|
---|
| 576 |
|
---|
| 577 | template<typename _MatrixType, typename OrderingType, typename Rhs>
|
---|
| 578 | struct solve_retval<SparseQR<_MatrixType,OrderingType>, Rhs>
|
---|
| 579 | : solve_retval_base<SparseQR<_MatrixType,OrderingType>, Rhs>
|
---|
| 580 | {
|
---|
| 581 | typedef SparseQR<_MatrixType,OrderingType> Dec;
|
---|
| 582 | EIGEN_MAKE_SOLVE_HELPERS(Dec,Rhs)
|
---|
| 583 |
|
---|
| 584 | template<typename Dest> void evalTo(Dest& dst) const
|
---|
| 585 | {
|
---|
| 586 | dec()._solve(rhs(),dst);
|
---|
| 587 | }
|
---|
| 588 | };
|
---|
| 589 | template<typename _MatrixType, typename OrderingType, typename Rhs>
|
---|
| 590 | struct sparse_solve_retval<SparseQR<_MatrixType, OrderingType>, Rhs>
|
---|
| 591 | : sparse_solve_retval_base<SparseQR<_MatrixType, OrderingType>, Rhs>
|
---|
| 592 | {
|
---|
| 593 | typedef SparseQR<_MatrixType, OrderingType> Dec;
|
---|
| 594 | EIGEN_MAKE_SPARSE_SOLVE_HELPERS(Dec, Rhs)
|
---|
| 595 |
|
---|
| 596 | template<typename Dest> void evalTo(Dest& dst) const
|
---|
| 597 | {
|
---|
| 598 | this->defaultEvalTo(dst);
|
---|
| 599 | }
|
---|
| 600 | };
|
---|
| 601 | } // end namespace internal
|
---|
| 602 |
|
---|
| 603 | template <typename SparseQRType, typename Derived>
|
---|
| 604 | struct SparseQR_QProduct : ReturnByValue<SparseQR_QProduct<SparseQRType, Derived> >
|
---|
| 605 | {
|
---|
| 606 | typedef typename SparseQRType::QRMatrixType MatrixType;
|
---|
| 607 | typedef typename SparseQRType::Scalar Scalar;
|
---|
| 608 | typedef typename SparseQRType::Index Index;
|
---|
| 609 | // Get the references
|
---|
| 610 | SparseQR_QProduct(const SparseQRType& qr, const Derived& other, bool transpose) :
|
---|
| 611 | m_qr(qr),m_other(other),m_transpose(transpose) {}
|
---|
| 612 | inline Index rows() const { return m_transpose ? m_qr.rows() : m_qr.cols(); }
|
---|
| 613 | inline Index cols() const { return m_other.cols(); }
|
---|
| 614 |
|
---|
| 615 | // Assign to a vector
|
---|
| 616 | template<typename DesType>
|
---|
| 617 | void evalTo(DesType& res) const
|
---|
| 618 | {
|
---|
| 619 | Index m = m_qr.rows();
|
---|
| 620 | Index n = m_qr.cols();
|
---|
| 621 | Index diagSize = (std::min)(m,n);
|
---|
| 622 | res = m_other;
|
---|
| 623 | if (m_transpose)
|
---|
| 624 | {
|
---|
| 625 | eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
|
---|
| 626 | //Compute res = Q' * other column by column
|
---|
| 627 | for(Index j = 0; j < res.cols(); j++){
|
---|
| 628 | for (Index k = 0; k < diagSize; k++)
|
---|
| 629 | {
|
---|
| 630 | Scalar tau = Scalar(0);
|
---|
| 631 | tau = m_qr.m_Q.col(k).dot(res.col(j));
|
---|
| 632 | if(tau==Scalar(0)) continue;
|
---|
| 633 | tau = tau * m_qr.m_hcoeffs(k);
|
---|
| 634 | res.col(j) -= tau * m_qr.m_Q.col(k);
|
---|
| 635 | }
|
---|
| 636 | }
|
---|
| 637 | }
|
---|
| 638 | else
|
---|
| 639 | {
|
---|
| 640 | eigen_assert(m_qr.m_Q.rows() == m_other.rows() && "Non conforming object sizes");
|
---|
| 641 | // Compute res = Q * other column by column
|
---|
| 642 | for(Index j = 0; j < res.cols(); j++)
|
---|
| 643 | {
|
---|
| 644 | for (Index k = diagSize-1; k >=0; k--)
|
---|
| 645 | {
|
---|
| 646 | Scalar tau = Scalar(0);
|
---|
| 647 | tau = m_qr.m_Q.col(k).dot(res.col(j));
|
---|
| 648 | if(tau==Scalar(0)) continue;
|
---|
| 649 | tau = tau * m_qr.m_hcoeffs(k);
|
---|
| 650 | res.col(j) -= tau * m_qr.m_Q.col(k);
|
---|
| 651 | }
|
---|
| 652 | }
|
---|
| 653 | }
|
---|
| 654 | }
|
---|
| 655 |
|
---|
| 656 | const SparseQRType& m_qr;
|
---|
| 657 | const Derived& m_other;
|
---|
| 658 | bool m_transpose;
|
---|
| 659 | };
|
---|
| 660 |
|
---|
| 661 | template<typename SparseQRType>
|
---|
| 662 | struct SparseQRMatrixQReturnType : public EigenBase<SparseQRMatrixQReturnType<SparseQRType> >
|
---|
| 663 | {
|
---|
| 664 | typedef typename SparseQRType::Index Index;
|
---|
| 665 | typedef typename SparseQRType::Scalar Scalar;
|
---|
| 666 | typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
|
---|
| 667 | SparseQRMatrixQReturnType(const SparseQRType& qr) : m_qr(qr) {}
|
---|
| 668 | template<typename Derived>
|
---|
| 669 | SparseQR_QProduct<SparseQRType, Derived> operator*(const MatrixBase<Derived>& other)
|
---|
| 670 | {
|
---|
| 671 | return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(),false);
|
---|
| 672 | }
|
---|
| 673 | SparseQRMatrixQTransposeReturnType<SparseQRType> adjoint() const
|
---|
| 674 | {
|
---|
| 675 | return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
|
---|
| 676 | }
|
---|
| 677 | inline Index rows() const { return m_qr.rows(); }
|
---|
| 678 | inline Index cols() const { return (std::min)(m_qr.rows(),m_qr.cols()); }
|
---|
| 679 | // To use for operations with the transpose of Q
|
---|
| 680 | SparseQRMatrixQTransposeReturnType<SparseQRType> transpose() const
|
---|
| 681 | {
|
---|
| 682 | return SparseQRMatrixQTransposeReturnType<SparseQRType>(m_qr);
|
---|
| 683 | }
|
---|
| 684 | template<typename Dest> void evalTo(MatrixBase<Dest>& dest) const
|
---|
| 685 | {
|
---|
| 686 | dest.derived() = m_qr.matrixQ() * Dest::Identity(m_qr.rows(), m_qr.rows());
|
---|
| 687 | }
|
---|
| 688 | template<typename Dest> void evalTo(SparseMatrixBase<Dest>& dest) const
|
---|
| 689 | {
|
---|
| 690 | Dest idMat(m_qr.rows(), m_qr.rows());
|
---|
| 691 | idMat.setIdentity();
|
---|
| 692 | // Sort the sparse householder reflectors if needed
|
---|
| 693 | const_cast<SparseQRType *>(&m_qr)->sort_matrix_Q();
|
---|
| 694 | dest.derived() = SparseQR_QProduct<SparseQRType, Dest>(m_qr, idMat, false);
|
---|
| 695 | }
|
---|
| 696 |
|
---|
| 697 | const SparseQRType& m_qr;
|
---|
| 698 | };
|
---|
| 699 |
|
---|
| 700 | template<typename SparseQRType>
|
---|
| 701 | struct SparseQRMatrixQTransposeReturnType
|
---|
| 702 | {
|
---|
| 703 | SparseQRMatrixQTransposeReturnType(const SparseQRType& qr) : m_qr(qr) {}
|
---|
| 704 | template<typename Derived>
|
---|
| 705 | SparseQR_QProduct<SparseQRType,Derived> operator*(const MatrixBase<Derived>& other)
|
---|
| 706 | {
|
---|
| 707 | return SparseQR_QProduct<SparseQRType,Derived>(m_qr,other.derived(), true);
|
---|
| 708 | }
|
---|
| 709 | const SparseQRType& m_qr;
|
---|
| 710 | };
|
---|
| 711 |
|
---|
| 712 | } // end namespace Eigen
|
---|
| 713 |
|
---|
| 714 | #endif
|
---|